diff --git a/.gitignore b/.gitignore
index d35aaa9efd4a75f08c9a4976565abc5ceb9559c9..54bbb139f7ddb0d72a14b276157237799dae2d15 100755
--- a/.gitignore
+++ b/.gitignore
@@ -1,13 +1,12 @@
-pps_ws/build/
-pps_ws/devel/
+dfall_ws/build/
+dfall_ws/devel/
-pps_ws/src/d_fall_pps/lib/vicon/
-pps_ws/src/d_fall_pps/include/DataStreamClient.h
+dfall_ws/src/dfall_pkg/lib/vicon/
+dfall_ws/src/dfall_pkg/include/DataStreamClient.h
-pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/*.pro.user
-
-pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/*.pro.user
-pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/*.pro.user.*
+# Qt Project files with user preferences
+*.pro.user
+*.pro.user.*
*.pyc
*.orig
diff --git a/pps_ws/.catkin_workspace b/dfall_ws/.catkin_workspace
similarity index 100%
rename from pps_ws/.catkin_workspace
rename to dfall_ws/.catkin_workspace
diff --git a/pps_ws/src/CMakeLists.txt b/dfall_ws/src/CMakeLists.txt
similarity index 100%
rename from pps_ws/src/CMakeLists.txt
rename to dfall_ws/src/CMakeLists.txt
diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/dfall_ws/src/dfall_pkg/CMakeLists.txt
similarity index 63%
rename from pps_ws/src/d_fall_pps/CMakeLists.txt
rename to dfall_ws/src/dfall_pkg/CMakeLists.txt
index 222de5fae7dff4b3a96b1eecb8125a261bbd8b9c..094f23d60237fe60102fa9c27e2b4e63aafdcf3c 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/dfall_ws/src/dfall_pkg/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.0)
-project(d_fall_pps)
+project(dfall_pkg)
## Add support for C++11, supported in ROS Kinetic and newer
# add_definitions(-std=c++11)
@@ -57,16 +57,10 @@ find_package(Qt5Svg REQUIRED)
# GUI -- Add src, includes, and resources
-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)
-set(MY_RESOURCE_FILE_QRC ${PROJECT_SOURCE_DIR}/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.qrc)
-
-
-
-# StudentGUI -- Add src, includes, and resources
-set(STUDENT_GUI_LIB_PATH_SRC ${PROJECT_SOURCE_DIR}/GUI_Qt/studentGUI/src)
-set(STUDENT_GUI_LIB_PATH_INC ${PROJECT_SOURCE_DIR}/GUI_Qt/studentGUI/include)
-set(STUDENT_RESOURCE_FILE_QRC ${PROJECT_SOURCE_DIR}/GUI_Qt/studentGUI/studentgui.qrc)
+set(SYSTEM_CONFIG_GUI_LIB_PATH_SRC ${PROJECT_SOURCE_DIR}/GUI_Qt/systemConfigGUI/src)
+set(SYSTEM_CONFIG_GUI_LIB_PATH_INC ${PROJECT_SOURCE_DIR}/GUI_Qt/systemConfigGUI/include)
+set(SYSTEM_CONFIG_GUI_LIB_PATH_FORMS ${PROJECT_SOURCE_DIR}/GUI_Qt/systemConfigGUI/forms)
+set(SYSTEM_CONFIG_GUI_RESOURCE_FILE_QRC ${PROJECT_SOURCE_DIR}/GUI_Qt/systemConfigGUI/systemconfiggui.qrc)
@@ -91,31 +85,19 @@ add_definitions(-std=c++11)
# GUI -- Special Qt sources that need to be wrapped before being compiled
# they have the Qt macro QOBJECT inside, the MOC cpp file needs to be done manually
set(SRC_HDRS_QOBJECT_GUI
- ${MY_GUI_LIB_PATH_INC}/crazyFlyZoneTab.h
- ${MY_GUI_LIB_PATH_INC}/myGraphicsScene.h
- ${MY_GUI_LIB_PATH_INC}/myGraphicsView.h
- ${MY_GUI_LIB_PATH_INC}/mainguiwindow.h
- ${MY_GUI_LIB_PATH_INC}/rosNodeThread_for_managerGUI.h
- ${MY_GUI_LIB_PATH_INC}/CFLinker.h
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_INC}/crazyFlyZoneTab.h
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_INC}/myGraphicsScene.h
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_INC}/myGraphicsView.h
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_INC}/mainguiwindow.h
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_INC}/rosNodeThread_for_systemConfigGUI.h
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_INC}/CFLinker.h
)
# GUI -- wrap UI file and QOBJECT files
-qt5_wrap_ui(UIS_HDRS_GUI ${MY_GUI_LIB_PATH_SRC}/mainguiwindow.ui)
+qt5_wrap_ui(UIS_HDRS_GUI ${SYSTEM_CONFIG_GUI_LIB_PATH_FORMS}/mainguiwindow.ui)
qt5_wrap_cpp(SRC_MOC_HDRS_GUI ${SRC_HDRS_QOBJECT_GUI})
# GUI -- wrap resource file qrc->rcc
-qt5_add_resources(MY_RESOURCE_FILE_RCC ${MY_RESOURCE_FILE_QRC})
-
-
+qt5_add_resources(SYSTEM_CONFIG_GUI_RESOURCE_FILE_RRC ${SYSTEM_CONFIG_GUI_RESOURCE_FILE_QRC})
-# StudentGUI -- Special Qt sources that need to be wrapped before being compiled
-set(SRC_HDRS_QOBJECT_STUDENT_GUI
- ${STUDENT_GUI_LIB_PATH_INC}/MainWindow.h
- ${STUDENT_GUI_LIB_PATH_INC}/rosNodeThread_for_studentGUI.h
- )
-# StudentGUI -- wrap UI file and QOBJECT files
-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})
-# GUI -- wrap resource file qrc->rcc
-qt5_add_resources(STUDENT_RESOURCE_FILE_RCC ${STUDENT_RESOURCE_FILE_QRC})
@@ -135,7 +117,9 @@ set(SRC_HDRS_QOBJECT_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_INC}/rosNodeThread_for_flyingAgentGUI.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/safecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/studentcontrollertab.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/templatecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/topbanner.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/tuningcontrollertab.h
)
# Flying Agent GUI -- wrap UI file and QOBJECT files
qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
@@ -149,7 +133,9 @@ qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/pickercontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/safecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/studentcontrollertab.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/templatecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/topbanner.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/tuningcontrollertab.ui
)
qt5_wrap_cpp(SRC_MOC_HDRS_FLYING_AGENT_GUI ${SRC_HDRS_QOBJECT_FLYING_AGENT_GUI})
# Flying Agent GUI -- wrap resource file qrc->rcc
@@ -205,7 +191,13 @@ add_message_files(
SetpointV2.msg
CrazyflieEntry.msg
CrazyflieDB.msg
- #----------------------------------------------------------------------
+ #------------------------
+ IntWithHeader.msg
+ FloatWithHeader.msg
+ StringWithHeader.msg
+ SetpointWithHeader.msg
+ CustomButtonWithHeader.msg
+ #------------------------
DebugMsg.msg
CustomButton.msg
ViconSubscribeObjectName.msg
@@ -219,12 +211,14 @@ add_message_files(
# )
add_service_files(
FILES
- LoadYamlFiles.srv
+ IntIntService.srv
Controller.srv
CMRead.srv
CMQuery.srv
CMUpdate.srv
CMCommand.srv
+ LoadYamlFromFilename.srv
+ GetSetpointService.srv
)
## Generate actions in the 'action' folder
@@ -274,9 +268,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 ${FLYING_AGENT_GUI_LIB_PATH_INC} # FlyingAgentGUI -- include headers from GUI in package
+ INCLUDE_DIRS include ${SYSTEM_CONFIG_GUI_LIB_PATH_INC} # SystemConfigGUI -- include headers from GUI in package
+ INCLUDE_DIRS include ${FLYING_AGENT_GUI_LIB_PATH_INC} # FlyingAgentGUI -- include headers from GUI in package
LIBRARIES
CATKIN_DEPENDS roscpp rospy std_msgs rosbag roslib
DEPENDS
@@ -290,9 +283,8 @@ catkin_package(
## Your package locations should be listed before other locations
# 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
- ${FLYING_AGENT_GUI_LIB_PATH_INC} # FlyingAgentGUI -- include directory inside GUI folder
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_INC} # SystemConfigGUI -- include directory inside GUI folder
+ ${FLYING_AGENT_GUI_LIB_PATH_INC} # FlyingAgentGUI -- include directory inside GUI folder
${catkin_INCLUDE_DIRS}
include
include/nodes
@@ -311,57 +303,60 @@ include_directories(
## Declare a C++ executable
## 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(${PROJECT_NAME}_node src/dfall_pkg_node.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(PickerControllerService src/nodes/PickerControllerService.cpp)
-add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
-add_executable(ParameterService src/nodes/ParameterService.cpp)
+add_executable(FlyingAgentClient src/nodes/FlyingAgentClient.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(DefaultControllerService src/nodes/DefaultControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
+add_executable(DemoControllerService src/nodes/DemoControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(StudentControllerService src/nodes/StudentControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
+add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
+add_executable(TuningControllerService src/nodes/TuningControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(PickerControllerService src/nodes/PickerControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(TemplateControllerService src/nodes/TemplateControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
+add_executable(ParameterService src/nodes/ParameterService.cpp)
# GUI -- Add sources here
-set(MY_CPP_SOURCES_GUI # compilation of sources
- ${MY_GUI_LIB_PATH_SRC}/mainguiwindow.cpp
- ${MY_GUI_LIB_PATH_SRC}/main.cpp
- ${MY_GUI_LIB_PATH_SRC}/cornergrabber.cpp
- ${MY_GUI_LIB_PATH_SRC}/crazyFlyZone.cpp
- ${MY_GUI_LIB_PATH_SRC}/crazyFlyZoneTab.cpp
- ${MY_GUI_LIB_PATH_SRC}/myGraphicsRectItem.cpp
- ${MY_GUI_LIB_PATH_SRC}/myGraphicsScene.cpp
- ${MY_GUI_LIB_PATH_SRC}/myGraphicsView.cpp
- ${MY_GUI_LIB_PATH_SRC}/tablePiece.cpp
- ${MY_GUI_LIB_PATH_SRC}/marker.cpp
- ${MY_GUI_LIB_PATH_SRC}/rosNodeThread_for_managerGUI.cpp
- ${MY_GUI_LIB_PATH_SRC}/crazyFly.cpp
- ${MY_GUI_LIB_PATH_SRC}/CFLinker.cpp
- ${MY_GUI_LIB_PATH_SRC}/channelLUT.cpp
- ${MY_GUI_LIB_PATH_SRC}/centerMarker.cpp
+set(SYSTEM_CONFIG_GUI_CPP_SOURCES # compilation of sources
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/mainguiwindow.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/main.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/cornergrabber.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/crazyFlyZone.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/crazyFlyZoneTab.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/myGraphicsRectItem.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/myGraphicsScene.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/myGraphicsView.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/tablePiece.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/marker.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/rosNodeThread_for_systemConfigGUI.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/crazyFly.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/CFLinker.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/channelLUT.cpp
+ ${SYSTEM_CONFIG_GUI_LIB_PATH_SRC}/centerMarker.cpp
)
-# StudentGUI -- Add sources here
-set(MY_CPP_SOURCES_STUDENT_GUI # compilation of sources
- ${STUDENT_GUI_LIB_PATH_SRC}/MainWindow.cpp
- ${STUDENT_GUI_LIB_PATH_SRC}/main.cpp
- ${STUDENT_GUI_LIB_PATH_SRC}/rosNodeThread_for_studentGUI.cpp
- )
-
-
# FLYING AGENT GUI -- Add sources here
-set(MY_CPP_SOURCES_FLYING_AGENT_GUI # compilation of sources
+set(FLYING_AGENT_GUI_CPP_SOURCES # compilation of sources
${FLYING_AGENT_GUI_LIB_PATH_SRC}/mainwindow.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/main.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/rosNodeThread_for_flyingAgentGUI.cpp
@@ -374,57 +369,52 @@ set(MY_CPP_SOURCES_FLYING_AGENT_GUI # compilation of sources
${FLYING_AGENT_GUI_LIB_PATH_SRC}/pickercontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/safecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/studentcontrollertab.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/templatecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/topbanner.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/tuningcontrollertab.cpp
)
# 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)
+add_executable(SystemConfigGUI ${SYSTEM_CONFIG_GUI_CPP_SOURCES} ${UIS_HDRS_GUI} ${SRC_MOC_HDRS_GUI} ${SYSTEM_CONFIG_GUI_RESOURCE_FILE_RRC})
+qt5_use_modules(SystemConfigGUI Widgets)
-# StudentGUI -- Add executables here
-add_executable(student_GUI ${MY_CPP_SOURCES_STUDENT_GUI} ${UIS_HDRS_STUDENT_GUI} ${SRC_MOC_HDRS_STUDENT_GUI} ${STUDENT_RESOURCE_FILE_RCC})
-qt5_use_modules(student_GUI Widgets)
-
-
# FLYING AGENT GUI -- Add executables here
-add_executable(flyingAgentGUI ${MY_CPP_SOURCES_FLYING_AGENT_GUI} ${UIS_HDRS_FLYING_AGENT_GUI} ${SRC_MOC_HDRS_FLYING_AGENT_GUI} ${FLYING_AGENT_GUI_RESOURCE_FILE_RCC})
-qt5_use_modules(flyingAgentGUI Widgets)
+add_executable(FlyingAgentGUI ${FLYING_AGENT_GUI_CPP_SOURCES} ${UIS_HDRS_FLYING_AGENT_GUI} ${SRC_MOC_HDRS_FLYING_AGENT_GUI} ${FLYING_AGENT_GUI_RESOURCE_FILE_RCC})
+qt5_use_modules(FlyingAgentGUI Widgets)
if(VICON_LIBRARY)
- add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+ add_dependencies(ViconDataPublisher dfall_pkg_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(PickerControllerService 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(FlyingAgentClient dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(BatteryMonitor dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(DefaultControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(SafeControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(DemoControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(StudentControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(MpcControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(RemoteControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(TuningControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(PickerControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(TemplateControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(CentralManagerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(ParameterService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
# GUI-- dependencies
-add_dependencies(my_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-
-
-
-# StudentGUI-- dependencies
-add_dependencies(student_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(SystemConfigGUI dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
# FLYING AGENT GUI-- dependencies
-add_dependencies(flyingAgentGUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(FlyingAgentGUI dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -450,42 +440,36 @@ if(VICON_LIBRARY)
target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
endif()
-target_link_libraries(PPSClient ${catkin_LIBRARIES})
-target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
-target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
-target_link_libraries(StudentControllerService ${catkin_LIBRARIES})
+target_link_libraries(FlyingAgentClient ${catkin_LIBRARIES})
+target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
+target_link_libraries(DefaultControllerService ${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(PickerControllerService ${catkin_LIBRARIES})
-target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
-target_link_libraries(ParameterService ${catkin_LIBRARIES})
+target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
+target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
+target_link_libraries(PickerControllerService ${catkin_LIBRARIES})
+target_link_libraries(TemplateControllerService ${catkin_LIBRARIES})
+target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
+target_link_libraries(ParameterService ${catkin_LIBRARIES})
# GUI -- link libraries
-target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff
-target_link_libraries(my_GUI Qt5::Svg)
-
-# target_link_libraries(my_library Qt5::Widgets) # GUI -- let my_library have access to Qt stuff
-# target_link_libraries(my_GUI my_library)
-target_link_libraries(my_GUI ${catkin_LIBRARIES})
-
-
-
-# StudentGUI -- link libraries
-target_link_libraries(student_GUI Qt5::Widgets) # GUI -- let student_GUI have acesss to Qt stuff
-target_link_libraries(student_GUI ${catkin_LIBRARIES})
+target_link_libraries(SystemConfigGUI Qt5::Widgets) # GUI -- let SystemConfigGUI have acesss to Qt stuff
+target_link_libraries(SystemConfigGUI Qt5::Svg)
+target_link_libraries(SystemConfigGUI ${catkin_LIBRARIES})
# Flying Agent GUI -- link libraries
-target_link_libraries(flyingAgentGUI Qt5::Widgets) # GUI -- let flyingAgentGUI have acesss to Qt stuff
-target_link_libraries(flyingAgentGUI ${catkin_LIBRARIES})
+target_link_libraries(FlyingAgentGUI Qt5::Widgets) # GUI -- let FlyingAgentGUI have acesss to Qt stuff
+target_link_libraries(FlyingAgentGUI ${catkin_LIBRARIES})
@@ -529,7 +513,7 @@ target_link_libraries(flyingAgentGUI ${catkin_LIBRARIES})
#############
## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_d_fall_pps.cpp)
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_dfall_pkg.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
similarity index 79%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
index 07c4fbced43644193b0a431f2cf4d0b465220a11..529942c8ac112c59a1de472de706e7ef463a0986 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
@@ -27,7 +27,10 @@ SOURCES += src/main.cpp\
src/coordinatorrow.cpp \
src/studentcontrollertab.cpp \
src/defaultcontrollertab.cpp \
- src/pickercontrollertab.cpp
+ src/pickercontrollertab.cpp \
+ src/templatecontrollertab.cpp \
+ src/tuningcontrollertab.cpp \
+
HEADERS += include/mainwindow.h \
include/topbanner.h \
@@ -39,7 +42,12 @@ HEADERS += include/mainwindow.h \
include/coordinatorrow.h \
include/studentcontrollertab.h \
include/defaultcontrollertab.h \
- include/pickercontrollertab.h
+ include/pickercontrollertab.h \
+ include/templatecontrollertab.h \
+ include/tuningcontrollertab.h \
+ include/Constants_for_Qt_compile.h \
+
+
FORMS += forms/mainwindow.ui \
forms/topbanner.ui \
@@ -51,7 +59,10 @@ FORMS += forms/mainwindow.ui \
forms/coordinatorrow.ui \
forms/studentcontrollertab.ui \
forms/defaultcontrollertab.ui \
- forms/pickercontrollertab.ui
+ forms/pickercontrollertab.ui \
+ forms/templatecontrollertab.ui \
+ forms/tuningcontrollertab.ui \
+
RESOURCES += \
flyingagentgui.qrc
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/flyingagentgui.qrc b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingagentgui.qrc
similarity index 82%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/flyingagentgui.qrc
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingagentgui.qrc
index 0f6e7d8eebb915c0f0f16522394150bbc2d84ec1..abfa96423fda8d142147561ebe3bb48191b2dac3 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/flyingagentgui.qrc
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingagentgui.qrc
@@ -6,14 +6,17 @@
<file>images/battery_80.png</file>
<file>images/battery_empty.png</file>
<file>images/battery_full.png</file>
+ <file>images/battery_unknown.png</file>
+ <file>images/battery_unavailable.png</file>
<file>images/rf_connected.png</file>
<file>images/rf_connecting.png</file>
<file>images/rf_disconnected.png</file>
- <file>images/battery_unknown.png</file>
<file>images/flying_state_disabling.png</file>
<file>images/flying_state_enabling.png</file>
<file>images/flying_state_flying.png</file>
<file>images/flying_state_off.png</file>
<file>images/flying_state_unknown.png</file>
+ <file>images/flying_state_unavailable.png</file>
+ <file>images/emergency_stop.png</file>
</qresource>
-</RCC>
+</RCC>
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/connectstartstopbar.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/connectstartstopbar.ui
similarity index 87%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/connectstartstopbar.ui
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/connectstartstopbar.ui
index c08c3341678b5e68eb2f9d7738f786fb1a9be3c2..8c1b2fa4bc1e3a5a0e3ee10aaa2432c55a5432f4 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/connectstartstopbar.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/connectstartstopbar.ui
@@ -22,19 +22,19 @@
<item row="0" column="0">
<layout class="QHBoxLayout" name="horizontalLayout">
<property name="spacing">
- <number>6</number>
+ <number>12</number>
</property>
<property name="leftMargin">
<number>6</number>
</property>
<property name="topMargin">
- <number>6</number>
+ <number>0</number>
</property>
<property name="rightMargin">
<number>6</number>
</property>
<property name="bottomMargin">
- <number>6</number>
+ <number>0</number>
</property>
<item>
<widget class="QPushButton" name="rf_disconnect_button">
@@ -47,13 +47,13 @@
<property name="minimumSize">
<size>
<width>0</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>750</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="text">
@@ -71,8 +71,8 @@
</property>
<property name="minimumSize">
<size>
- <width>95</width>
- <height>70</height>
+ <width>72</width>
+ <height>55</height>
</size>
</property>
<property name="maximumSize">
@@ -97,13 +97,13 @@
<property name="minimumSize">
<size>
<width>0</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>750</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="text">
@@ -113,22 +113,25 @@
</item>
<item>
<widget class="QLineEdit" name="battery_voltage_lineEdit">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
<property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>180</width>
- <height>70</height>
+ <width>0</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="font">
@@ -144,6 +147,9 @@
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
</widget>
</item>
<item>
@@ -156,8 +162,8 @@
</property>
<property name="minimumSize">
<size>
- <width>50</width>
- <height>70</height>
+ <width>38</width>
+ <height>55</height>
</size>
</property>
<property name="maximumSize">
@@ -172,7 +178,7 @@
</widget>
</item>
<item>
- <widget class="QPushButton" name="take_off_button">
+ <widget class="QPushButton" name="enable_flying_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -182,13 +188,13 @@
<property name="minimumSize">
<size>
<width>0</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>750</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="text">
@@ -197,7 +203,7 @@
</widget>
</item>
<item>
- <widget class="QPushButton" name="land_button">
+ <widget class="QPushButton" name="disable_flying_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -207,13 +213,13 @@
<property name="minimumSize">
<size>
<width>0</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>750</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="text">
@@ -231,8 +237,8 @@
</property>
<property name="minimumSize">
<size>
- <width>90</width>
- <height>70</height>
+ <width>72</width>
+ <height>55</height>
</size>
</property>
<property name="maximumSize">
@@ -257,13 +263,13 @@
<property name="minimumSize">
<size>
<width>0</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>750</width>
- <height>70</height>
+ <height>50</height>
</size>
</property>
<property name="font">
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
similarity index 66%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
index cfd93a07775814565b809f542948599d0a0d65c9..471a55f3a30fb3afa2075720f8a66d6dfaf53a66 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
@@ -28,7 +28,10 @@
<item row="0" column="0">
<widget class="QTabWidget" name="controller_tabs_widget">
<property name="currentIndex">
- <number>2</number>
+ <number>4</number>
+ </property>
+ <property name="movable">
+ <bool>true</bool>
</property>
<widget class="QWidget" name="default_tab">
<attribute name="title">
@@ -36,7 +39,7 @@
</attribute>
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
- <widget class="DefaultControllerTab" name="widget" native="true"/>
+ <widget class="DefaultControllerTab" name="default_controller_tab_widget" native="true"/>
</item>
</layout>
</widget>
@@ -46,7 +49,7 @@
</attribute>
<layout class="QGridLayout" name="gridLayout_4">
<item row="0" column="0">
- <widget class="StudentControllerTab" name="widget_3" native="true"/>
+ <widget class="StudentControllerTab" name="student_controller_tab_widget" native="true"/>
</item>
</layout>
</widget>
@@ -56,17 +59,27 @@
</attribute>
<layout class="QGridLayout" name="gridLayout_5">
<item row="0" column="0">
- <widget class="PickerControllerTab" name="widget_4" native="true"/>
+ <widget class="PickerControllerTab" name="picker_controller_tab_widget" native="true"/>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="tuning_tab">
+ <attribute name="title">
+ <string>Tuning</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="0" column="0">
+ <widget class="TuningControllerTab" name="tuning_controller_tab_widget" native="true"/>
</item>
</layout>
</widget>
- <widget class="QWidget" name="safe_tab">
+ <widget class="QWidget" name="template_tab">
<attribute name="title">
- <string>Safe</string>
+ <string>Template</string>
</attribute>
- <layout class="QGridLayout" name="gridLayout_3">
+ <layout class="QGridLayout" name="gridLayout_7">
<item row="0" column="0">
- <widget class="SafeControllerTab" name="widget_2" native="true"/>
+ <widget class="TemplateControllerTab" name="template_controller_tab_widget" native="true"/>
</item>
</layout>
</widget>
@@ -75,12 +88,6 @@
</layout>
</widget>
<customwidgets>
- <customwidget>
- <class>SafeControllerTab</class>
- <extends>QWidget</extends>
- <header>safecontrollertab.h</header>
- <container>1</container>
- </customwidget>
<customwidget>
<class>StudentControllerTab</class>
<extends>QWidget</extends>
@@ -99,6 +106,18 @@
<header>pickercontrollertab.h</header>
<container>1</container>
</customwidget>
+ <customwidget>
+ <class>TuningControllerTab</class>
+ <extends>QWidget</extends>
+ <header>tuningcontrollertab.h</header>
+ <container>1</container>
+ </customwidget>
+ <customwidget>
+ <class>TemplateControllerTab</class>
+ <extends>QWidget</extends>
+ <header>templatecontrollertab.h</header>
+ <container>1</container>
+ </customwidget>
</customwidgets>
<resources/>
<connections/>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/coordinator.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/coordinator.ui
similarity index 74%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/coordinator.ui
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/coordinator.ui
index 13377061ad3943ba1e1a7d30aa819b11c0940384..855f9cecc650c5587bd7b3fb484c96ad006f294a 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/coordinator.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/coordinator.ui
@@ -21,6 +21,12 @@
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QLabel" name="coordintor_title_label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="font">
<font>
<weight>75</weight>
@@ -37,6 +43,12 @@
</item>
<item>
<widget class="QLabel" name="coordinator_id_label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="font">
<font>
<weight>75</weight>
@@ -58,10 +70,17 @@
</property>
</widget>
</item>
+ <item>
+ <widget class="QPushButton" name="toggle_details_button">
+ <property name="text">
+ <string>Toggle Details</string>
+ </property>
+ </widget>
+ </item>
<item>
<widget class="QPushButton" name="delete_button">
<property name="text">
- <string>Delete</string>
+ <string>Remove All</string>
</property>
</widget>
</item>
@@ -85,6 +104,12 @@
</item>
<item>
<widget class="QScrollArea" name="coordinated_agents_scrollArea">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Expanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="widgetResizable">
<bool>true</bool>
</property>
@@ -93,8 +118,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>583</width>
- <height>854</height>
+ <width>565</width>
+ <height>622</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_for_coordinatedAgentsScrollArea"/>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/coordinatorrow.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/coordinatorrow.ui
similarity index 96%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/coordinatorrow.ui
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/coordinatorrow.ui
index 790ff9ce5c842373671285685f48c3b592cdb034..cba391caebc9b5451e11800ce6d8faf0be23d013 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/coordinatorrow.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/coordinatorrow.ui
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>660</width>
+ <width>740</width>
<height>45</height>
</rect>
</property>
@@ -55,14 +55,14 @@
<item>
<widget class="QCheckBox" name="shouldCoordinate_checkBox">
<property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>180</width>
+ <width>50</width>
<height>35</height>
</size>
</property>
@@ -81,7 +81,7 @@
<string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
</property>
<property name="text">
- <string>IDYYY , PPS_CFXX</string>
+ <string>IDYYY , CFXX</string>
</property>
<property name="checkable">
<bool>true</bool>
@@ -193,6 +193,9 @@
<property name="text">
<string>-.-- V</string>
</property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
</widget>
</item>
<item>
@@ -323,20 +326,20 @@
<item>
<widget class="QLabel" name="controller_enabled_label">
<property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>70</width>
+ <width>50</width>
<height>35</height>
</size>
</property>
<property name="maximumSize">
<size>
- <width>70</width>
+ <width>180</width>
<height>35</height>
</size>
</property>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/defaultcontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/defaultcontrollertab.ui
new file mode 100644
index 0000000000000000000000000000000000000000..c8225eaff2d6b6b9e9c3fc2b5d71237bce7b629c
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/defaultcontrollertab.ui
@@ -0,0 +1,1320 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>DefaultControllerTab</class>
+ <widget class="QWidget" name="DefaultControllerTab">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1614</width>
+ <height>991</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="1" column="0">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_current_state_title">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Current State:</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_current_state">
+ <property name="text">
+ <string>TextLabel</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="4" 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="2" column="0">
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item row="2" column="1">
+ <widget class="QLabel" name="label_row_x">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>x [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="6">
+ <widget class="QLabel" name="label_increment_title">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Increment</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_error_title">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Error</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLabel" name="label_row_yaw">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>yaw [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_error_title_line2">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>meas-ref</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QLabel" name="label_current_title_line2">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLabel" name="label_current_title">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Current</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="6">
+ <widget class="QLabel" name="label_increment_title_line2">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="label_row_y">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>y [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <widget class="QPushButton" name="y_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="y_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QPushButton" name="z_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="z_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QPushButton" name="yaw_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="yaw_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="QLabel" name="label_row_roll">
+ <property name="text">
+ <string>roll [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="3">
+ <widget class="QPushButton" name="default_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_measured_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Measured</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QLabel" name="label_row_pitch">
+ <property name="text">
+ <string>pitch [deg]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLabel" name="label_row_z">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <item>
+ <widget class="QPushButton" name="x_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="x_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_5">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QFrame" name="red_frame_position_left">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_measured_title_line2">
+ <property name="text">
+ <string>Position</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="red_frame_position_right">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_12">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="7" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_13">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="7">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="2" column="5">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="4">
+ <widget class="QLabel" name="label_new_title">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>New</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="4">
+ <widget class="QLabel" name="label_new_title_line2">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="4">
+ <widget class="QPushButton" name="set_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set New</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
similarity index 68%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
index 9304b2ad9c78438d2b92a9032e1fa6d67af9aa7d..c358d8befac4a77bff9b211af6cdb3eb9d2fd32c 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>595</width>
- <height>561</height>
+ <width>1572</width>
+ <height>403</height>
</rect>
</property>
<property name="sizePolicy">
@@ -26,174 +26,187 @@
</property>
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0">
- <layout class="QGridLayout" name="gridLayout">
- <item row="7" column="1">
- <widget class="QPushButton" name="load_yaml_safe_button">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="spacing">
+ <number>12</number>
+ </property>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="enable_student_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>40</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>40</height>
+ <height>50</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
<property name="text">
- <string>Safe</string>
+ <string>Student</string>
</property>
</widget>
</item>
- <item row="7" column="0">
- <widget class="QPushButton" name="enable_safe_button">
+ <item row="1" column="0">
+ <widget class="QLabel" name="load_yaml_label">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>40</height>
+ <width>0</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
- <width>180</width>
- <height>40</height>
+ <width>16777215</width>
+ <height>50</height>
</size>
</property>
<property name="font">
<font>
- <weight>50</weight>
- <bold>false</bold>
+ <weight>75</weight>
+ <bold>true</bold>
</font>
</property>
<property name="text">
- <string>Safe</string>
+ <string>Load YAML</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="0" column="0">
- <widget class="QLabel" name="enable_controller_top_label">
+ <item row="0" column="6">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="5">
+ <widget class="QPushButton" name="enable_template_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>20</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>20</height>
+ <height>50</height>
</size>
</property>
<property name="font">
<font>
- <weight>75</weight>
- <bold>true</bold>
+ <weight>50</weight>
+ <bold>false</bold>
</font>
</property>
<property name="text">
- <string>Enable</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <string>Template</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QLabel" name="load_yaml_top_label">
+ <item row="1" column="3">
+ <widget class="QPushButton" name="load_yaml_picker_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>20</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>20</height>
+ <height>50</height>
</size>
</property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
<property name="text">
- <string>Load</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <string>Picker</string>
</property>
</widget>
</item>
- <item row="4" column="1">
- <widget class="QPushButton" name="load_yaml_demo_button">
+ <item row="1" column="2">
+ <widget class="QPushButton" name="load_yaml_student_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>40</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>40</height>
+ <height>50</height>
</size>
</property>
<property name="text">
- <string>Demo</string>
+ <string>Student</string>
</property>
</widget>
</item>
- <item row="4" column="0">
- <widget class="QPushButton" name="enable_demo_button">
+ <item row="0" column="1">
+ <widget class="QPushButton" name="enable_default_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>40</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>40</height>
+ <height>50</height>
</size>
</property>
<property name="font">
@@ -203,41 +216,28 @@
</font>
</property>
<property name="text">
- <string>Demo</string>
+ <string>Default</string>
</property>
</widget>
</item>
- <item row="8" 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="1" column="0">
- <widget class="QLabel" name="enable_controller_bot_label">
+ <item row="0" column="0">
+ <widget class="QLabel" name="enable_controller_label">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>20</height>
+ <width>0</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
- <width>180</width>
- <height>20</height>
+ <width>16777215</width>
+ <height>50</height>
</size>
</property>
<property name="font">
@@ -247,156 +247,141 @@
</font>
</property>
<property name="text">
- <string>Controller</string>
+ <string>Enable</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QLabel" name="load_yaml_bot_label">
+ <item row="1" column="5">
+ <widget class="QPushButton" name="load_yaml_template_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>20</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>20</height>
+ <height>50</height>
</size>
</property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
<property name="text">
- <string>YAML</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <string>Template</string>
</property>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QPushButton" name="enable_default_button">
+ <item row="1" column="1">
+ <widget class="QPushButton" name="load_yaml_default_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>40</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>40</height>
+ <height>50</height>
</size>
</property>
- <property name="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
<property name="text">
<string>Default</string>
</property>
</widget>
</item>
- <item row="2" column="1">
- <widget class="QPushButton" name="load_yaml_default_button">
+ <item row="0" column="3">
+ <widget class="QPushButton" name="enable_picker_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>40</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>40</height>
+ <height>50</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
<property name="text">
- <string>Default</string>
+ <string>Picker</string>
</property>
</widget>
</item>
- <item row="3" column="0">
- <widget class="QPushButton" name="enable_student_button">
+ <item row="0" column="4">
+ <widget class="QPushButton" name="enable_tuning_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>40</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>40</height>
+ <height>50</height>
</size>
</property>
- <property name="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
<property name="text">
- <string>Student</string>
+ <string>Tuning</string>
</property>
</widget>
</item>
- <item row="3" column="1">
- <widget class="QPushButton" name="load_yaml_student_button">
+ <item row="1" column="4">
+ <widget class="QPushButton" name="load_yaml_tuning_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>120</width>
- <height>40</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>180</width>
- <height>40</height>
+ <height>50</height>
</size>
</property>
<property name="text">
- <string>Student</string>
+ <string>Tuning</string>
</property>
</widget>
</item>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
new file mode 100644
index 0000000000000000000000000000000000000000..e848317a9561d340ffe22e4156c7c15d4f270e0b
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
@@ -0,0 +1,352 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindow</class>
+ <widget class="QMainWindow" name="MainWindow">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1862</width>
+ <height>839</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>MainWindow</string>
+ </property>
+ <widget class="QWidget" name="centralWidget">
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <property name="spacing">
+ <number>6</number>
+ </property>
+ <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="Coordinator" name="customWidget_coordinator" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>300</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>800</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="Line" name="coordinator_to_main_panel_vertical_line">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <property name="spacing">
+ <number>6</number>
+ </property>
+ <property name="leftMargin">
+ <number>12</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="TopBanner" name="customWidget_topBanner" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <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>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="Line" name="line_1">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="ConnectStartStopBar" name="customWidget_connectStartStopBar" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <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>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="Line" name="line_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="EnableControllerLoadYamlBar" name="customWidget_enableControllerLoadYamlBar" native="true">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <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>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="Line" name="line_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="ControllerTabs" name="customWidget_controller_tabs" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QMenuBar" name="menuBar">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1862</width>
+ <height>40</height>
+ </rect>
+ </property>
+ <widget class="QMenu" name="menuFile">
+ <property name="title">
+ <string>File</string>
+ </property>
+ <addaction name="actionShowHide_Coordinator"/>
+ </widget>
+ <widget class="QMenu" name="menuLoad_YAML">
+ <property name="title">
+ <string>LoadYAML</string>
+ </property>
+ <addaction name="action_LoadYAML_BatteryMonitor"/>
+ <addaction name="action_LoadYAML_FlyingAgentClientConfig"/>
+ </widget>
+ <widget class="QMenu" name="menuControllers">
+ <property name="title">
+ <string>Controllers</string>
+ </property>
+ <addaction name="action_showHideController_default"/>
+ <addaction name="action_showHideController_student"/>
+ <addaction name="action_showHideController_picker"/>
+ <addaction name="action_showHideController_tuning"/>
+ <addaction name="action_showHideController_template"/>
+ </widget>
+ <addaction name="menuFile"/>
+ <addaction name="menuLoad_YAML"/>
+ <addaction name="menuControllers"/>
+ </widget>
+ <widget class="QToolBar" name="mainToolBar">
+ <attribute name="toolBarArea">
+ <enum>TopToolBarArea</enum>
+ </attribute>
+ <attribute name="toolBarBreak">
+ <bool>false</bool>
+ </attribute>
+ </widget>
+ <widget class="QStatusBar" name="statusBar"/>
+ <action name="actionShowHide_Coordinator">
+ <property name="text">
+ <string>Hide Coordinator</string>
+ </property>
+ </action>
+ <action name="action_LoadYAML_BatteryMonitor">
+ <property name="text">
+ <string>BatteryMonitor</string>
+ </property>
+ </action>
+ <action name="action_LoadYAML_FlyingAgentClientConfig">
+ <property name="text">
+ <string>FlyingAgentClientConfig</string>
+ </property>
+ </action>
+ <action name="action_showHideController_default">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </action>
+ <action name="action_showHideController_student">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Student</string>
+ </property>
+ </action>
+ <action name="action_showHideController_picker">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Picker</string>
+ </property>
+ </action>
+ <action name="action_showHideController_safe">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Safe</string>
+ </property>
+ </action>
+ <action name="action_showHideController_tuning">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Tuning</string>
+ </property>
+ </action>
+ <action name="action_showHideController_template">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Template</string>
+ </property>
+ </action>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <customwidgets>
+ <customwidget>
+ <class>TopBanner</class>
+ <extends>QWidget</extends>
+ <header location="global">topbanner.h</header>
+ <container>1</container>
+ </customwidget>
+ <customwidget>
+ <class>ConnectStartStopBar</class>
+ <extends>QWidget</extends>
+ <header location="global">connectstartstopbar.h</header>
+ <container>1</container>
+ </customwidget>
+ <customwidget>
+ <class>EnableControllerLoadYamlBar</class>
+ <extends>QWidget</extends>
+ <header>enablecontrollerloadyamlbar.h</header>
+ <container>1</container>
+ </customwidget>
+ <customwidget>
+ <class>ControllerTabs</class>
+ <extends>QWidget</extends>
+ <header>controllertabs.h</header>
+ <container>1</container>
+ </customwidget>
+ <customwidget>
+ <class>Coordinator</class>
+ <extends>QWidget</extends>
+ <header>coordinator.h</header>
+ <container>1</container>
+ </customwidget>
+ </customwidgets>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui
new file mode 100644
index 0000000000000000000000000000000000000000..7175ab3ce0a085ab519ed8d4baca5ec2cd3bef56
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui
@@ -0,0 +1,2597 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>PickerControllerTab</class>
+ <widget class="QWidget" name="PickerControllerTab">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1978</width>
+ <height>1312</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_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 row="1" column="0">
+ <widget class="Line" name="line_15">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="4" 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="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_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 row="13" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <item>
+ <widget class="QCheckBox" name="checkbox_standby">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="10" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_24">
+ <item>
+ <widget class="QCheckBox" name="checkbox_squat">
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_14">
+ <item>
+ <widget class="QCheckBox" name="checkbox_goto_start">
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="11" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_29">
+ <item>
+ <widget class="QCheckBox" name="checkbox_jump">
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="14" column="6">
+ <widget class="Line" name="line_19">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="14" column="3">
+ <widget class="Line" name="line_17">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="13" column="9">
+ <layout class="QHBoxLayout" name="horizontalLayout_41">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_standby_mass">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_minus_mass">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_plus_mass">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="14" column="8">
+ <widget class="Line" name="line_21">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="7">
+ <layout class="QHBoxLayout" name="horizontalLayout_28">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_lift_up_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_lift_up_inc_minus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_lift_up_inc_plus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="15" column="4">
+ <widget class="QLineEdit" name="lineEdit_increment_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="4">
+ <layout class="QHBoxLayout" name="horizontalLayout_13">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_goto_end_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_end_inc_minus_x">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_end_inc_plus_x">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="8" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_5">
+ <item>
+ <widget class="QPushButton" name="button_goto_end">
+ <property name="text">
+ <string>Goto End</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_goto_end_active">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="3">
+ <widget class="QLabel" name="label_smooth_column">
+ <property name="text">
+ <string>Smooth</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="15" column="6">
+ <widget class="QLineEdit" name="lineEdit_increment_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="9">
+ <layout class="QHBoxLayout" name="horizontalLayout_44">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_lift_up_mass">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_lift_up_inc_minus_mass">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_lift_up_inc_plus_mass">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="16" column="4">
+ <widget class="Line" name="line_10">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="17" column="6">
+ <widget class="QLineEdit" name="lineEdit_current_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="19" column="4">
+ <layout class="QHBoxLayout" name="horizontalLayout_20">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_x_unavailable">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="19" column="8">
+ <layout class="QHBoxLayout" name="horizontalLayout_36">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_yaw_unavailable">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="6">
+ <widget class="QLabel" name="label_y_column">
+ <property name="text">
+ <string>y[m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="7">
+ <layout class="QHBoxLayout" name="horizontalLayout_30">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_put_down_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_put_down_inc_minus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_put_down_inc_plus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="14" column="0">
+ <widget class="Line" name="line_16">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="14" column="4">
+ <widget class="Line" name="line_18">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="19" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_38">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_y_unavailable">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_5">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="19" column="7">
+ <layout class="QHBoxLayout" name="horizontalLayout_37">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_z_unavailable">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_4">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="16" column="8">
+ <widget class="Line" name="line_13">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="4">
+ <widget class="QLabel" name="label_x_column">
+ <property name="text">
+ <string>x[m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="7">
+ <layout class="QHBoxLayout" name="horizontalLayout_31">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_squat_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_squat_inc_minus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_squat_inc_plus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="8">
+ <layout class="QHBoxLayout" name="horizontalLayout_34">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_goto_start_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_start_inc_minus_yaw">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_start_inc_plus_yaw">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="17" column="0">
+ <widget class="QLabel" name="label_current">
+ <property name="text">
+ <string>Current:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_22">
+ <item>
+ <widget class="QCheckBox" name="checkbox_goto_end">
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <widget class="QPushButton" name="button_attach">
+ <property name="text">
+ <string>Attach</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_attach_active">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="7">
+ <layout class="QHBoxLayout" name="horizontalLayout_26">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_goto_start_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_start_inc_minus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_start_inc_plus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="14" column="9">
+ <widget class="Line" name="line_22">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="18" column="0">
+ <widget class="QLabel" name="label_error">
+ <property name="text">
+ <string>Error:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="19" column="0">
+ <widget class="QLabel" name="label_measured">
+ <property name="text">
+ <string>Measured:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="7">
+ <layout class="QHBoxLayout" name="horizontalLayout_32">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_jump_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_jump_inc_minus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_jump_inc_plus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="8" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_21">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_goto_end_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_end_inc_minus_y">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_end_inc_plus_y">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="9" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_23">
+ <item>
+ <widget class="QCheckBox" name="checkbox_put_down">
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="7" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_19">
+ <item>
+ <widget class="QCheckBox" name="checkbox_lift_up">
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="8" column="8">
+ <layout class="QHBoxLayout" name="horizontalLayout_35">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_goto_end_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_end_inc_minus_yaw">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_end_inc_plus_yaw">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="15" column="7">
+ <widget class="QLineEdit" name="lineEdit_increment_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="17" column="8">
+ <widget class="QLineEdit" name="lineEdit_current_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="7">
+ <widget class="QLabel" name="label_z_column">
+ <property name="text">
+ <string>z[m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <item>
+ <widget class="QPushButton" name="button_lift_up">
+ <property name="text">
+ <string>Lift Up</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_lift_up_active">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_function_column">
+ <property name="text">
+ <string>Function</string>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <item>
+ <widget class="QPushButton" name="button_squat">
+ <property name="text">
+ <string>Squaut</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_squat_active">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="7">
+ <layout class="QHBoxLayout" name="horizontalLayout_27">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_attach_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_attach_inc_minus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_attach_inc_plus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="17" column="9">
+ <widget class="QLineEdit" name="lineEdit_current_mass">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <item>
+ <widget class="QPushButton" name="button_put_down">
+ <property name="text">
+ <string>Put Down</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_put_down_active">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="17" column="4">
+ <widget class="QLineEdit" name="lineEdit_current_x">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="15" column="9">
+ <widget class="QLineEdit" name="lineEdit_increment_mass">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <item>
+ <widget class="QPushButton" name="button_jump">
+ <property name="text">
+ <string>Jump</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_jump_active">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="13" column="7">
+ <layout class="QHBoxLayout" name="horizontalLayout_25">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_standby_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_minus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_plus_z">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="14" column="7">
+ <widget class="Line" name="line_20">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="4">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_goto_start_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_start_inc_minus_x">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_start_inc_plus_x">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="16" column="3">
+ <widget class="Line" name="line_9">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="17" column="7">
+ <widget class="QLineEdit" name="lineEdit_current_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="16" column="9">
+ <widget class="Line" name="line_14">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="9">
+ <widget class="QLabel" name="label_mass_column">
+ <property name="text">
+ <string>mass[g]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="13" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_17">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_standby_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_minus_y">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_plus_y">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_18">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_goto_start_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_start_inc_minus_y">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_goto_start_inc_plus_y">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="13" column="8">
+ <layout class="QHBoxLayout" name="horizontalLayout_33">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_standby_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_minus_yaw">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_plus_yaw">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="16" column="6">
+ <widget class="Line" name="line_11">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="8">
+ <widget class="QLabel" name="label_yaw_column">
+ <property name="text">
+ <string>yaw[deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="16" column="7">
+ <widget class="Line" name="line_12">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="18" column="7">
+ <widget class="QLineEdit" name="lineEdit_error_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="18" column="8">
+ <widget class="QLineEdit" name="lineEdit_error_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_16">
+ <item>
+ <widget class="QCheckBox" name="checkbox_attach">
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="15" column="8">
+ <widget class="QLineEdit" name="lineEdit_increment_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="16" column="0">
+ <widget class="Line" name="line_8">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="13" column="4">
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_standby_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_minus_x">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_stanby_inc_plus_x">
+ <property name="maximumSize">
+ <size>
+ <width>40</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="15" column="0">
+ <widget class="QLabel" name="label_increment">
+ <property name="text">
+ <string>Increment:</string>
+ </property>
+ </widget>
+ </item>
+ <item row="13" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QPushButton" name="button_standby">
+ <property name="text">
+ <string>Standby</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_standby_active">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QPushButton" name="button_goto_start">
+ <property name="text">
+ <string>Goto Start</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_goto_start_active">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="17" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_15">
+ <item>
+ <widget class="QCheckBox" name="checkbox_current">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="18" column="4">
+ <widget class="QLineEdit" name="lineEdit_error_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="18" column="6">
+ <widget class="QLineEdit" name="lineEdit_error_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_12">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="topMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <widget class="QCheckBox" name="checkbox_should_publish_value_changed">
+ <property name="maximumSize">
+ <size>
+ <width>30</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_should_publish_value_changed">
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Publish setpoint change everytime a {x,y,z,yaw,mass} value is changed?</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="0">
+ <widget class="Line" name="line_37">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/safecontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/safecontrollertab.ui
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/safecontrollertab.ui
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/safecontrollertab.ui
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/studentcontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/studentcontrollertab.ui
new file mode 100644
index 0000000000000000000000000000000000000000..e3e70de43498c3add8cd56bf6c419f1759bf1a69
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/studentcontrollertab.ui
@@ -0,0 +1,1448 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>StudentControllerTab</class>
+ <widget class="QWidget" name="StudentControllerTab">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1692</width>
+ <height>931</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="12" column="0" rowspan="2">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <property name="horizontalSpacing">
+ <number>20</number>
+ </property>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="custom_button_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Button 2</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QPushButton" name="custom_button_4">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Button 4</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="lineEdit_custom_1">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="custom_button_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Button 3</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="custom_button_1">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Button 1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="5">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="4">
+ <widget class="QPushButton" name="custom_button_5">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Button 5</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="lineEdit_custom_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="lineEdit_custom_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QLineEdit" name="lineEdit_custom_4">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="4">
+ <widget class="QLineEdit" name="lineEdit_custom_5">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>240</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="9" column="0">
+ <layout class="QGridLayout" name="gridLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_measured_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Measured</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_5">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QFrame" name="red_frame_position_left">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_measured_title_line2">
+ <property name="text">
+ <string>Position</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="red_frame_position_right">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="label_row_x">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>x [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLabel" name="label_row_z">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QLabel" name="label_row_yaw">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>yaw [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QPushButton" name="x_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="x_increment_plus_button">
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="5">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLabel" name="label_row_y">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>y [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="QLabel" name="label_row_pitch">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>pitch [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="1">
+ <widget class="QLabel" name="label_row_roll">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>roll [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_error_title_line2">
+ <property name="text">
+ <string>meas-ref</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QLabel" name="label_current_title_2">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_error_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Error</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="3">
+ <widget class="QPushButton" name="default_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <widget class="QLabel" name="label_new_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>New</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="4">
+ <widget class="QLabel" name="label_new_title_line2">
+ <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="lineEdit_error_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLabel" name="label_current_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Current</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="6">
+ <widget class="QLabel" name="label_2">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Increment</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="4">
+ <widget class="QPushButton" name="set_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set New</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="6">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QPushButton" name="y_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="y_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <item>
+ <widget class="QPushButton" name="yaw_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="yaw_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <widget class="QPushButton" name="z_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="z_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="7">
+ <spacer name="horizontalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="7" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="8" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ <item row="14" 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="11" column="0">
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="10" column="0">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/templatecontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/templatecontrollertab.ui
new file mode 100644
index 0000000000000000000000000000000000000000..7ffe2b1781ec7d77fb97653c6e4f4428f514dbc7
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/templatecontrollertab.ui
@@ -0,0 +1,872 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>TemplateControllerTab</class>
+ <widget class="QWidget" name="TemplateControllerTab">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1335</width>
+ <height>937</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="2" column="0">
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="4" 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="0">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="topMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>6</number>
+ </property>
+ <item row="0" column="3">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="custom_button_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Button 2</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="custom_button_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Button 3</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="custom_button_1">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Button 1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_custom_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_custom_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_custom_1">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="3" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QLabel" name="label_new_title_line2">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLabel" name="label_row_x">
+ <property name="text">
+ <string>x [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLabel" name="label_new_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>New</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_measured_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Measured</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="label_row_y">
+ <property name="text">
+ <string>y [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_5">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QFrame" name="red_frame_position_left">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_measured_title_line2">
+ <property name="text">
+ <string>Position</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="red_frame_position_right">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLabel" name="label_row_z">
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLabel" name="label_row_yaw">
+ <property name="text">
+ <string>yaw [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="3">
+ <widget class="QPushButton" name="set_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set New</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_current_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Current</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QLabel" name="label_row_pitch">
+ <property name="text">
+ <string>pitch [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_current_title_line2">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="2">
+ <widget class="QPushButton" name="default_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="QLabel" name="label_row_roll">
+ <property name="text">
+ <string>roll [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="7" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_12">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/topbanner.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/topbanner.ui
similarity index 62%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/topbanner.ui
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/topbanner.ui
index 486f7235de0c57b4f08176daa3d7428627a2f2d1..75b32f76b0b6360a17143d4f9f94df63478908ce 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/topbanner.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/topbanner.ui
@@ -6,13 +6,13 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1049</width>
+ <width>1418</width>
<height>300</height>
</rect>
</property>
<property name="font">
<font>
- <pointsize>20</pointsize>
+ <pointsize>18</pointsize>
<weight>75</weight>
<bold>true</bold>
</font>
@@ -27,13 +27,13 @@
<number>6</number>
</property>
<property name="topMargin">
- <number>6</number>
+ <number>0</number>
</property>
<property name="rightMargin">
<number>6</number>
</property>
<property name="bottomMargin">
- <number>6</number>
+ <number>0</number>
</property>
<property name="spacing">
<number>6</number>
@@ -43,7 +43,7 @@
<property name="maximumSize">
<size>
<width>16777215</width>
- <height>120</height>
+ <height>100</height>
</size>
</property>
<property name="text">
@@ -54,6 +54,31 @@
</property>
</widget>
</item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="emergency_stop_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>ES</string>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
</layout>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tuningcontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tuningcontrollertab.ui
new file mode 100644
index 0000000000000000000000000000000000000000..28732632ec93dc1d1dba323e5aa054ac505d9da4
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tuningcontrollertab.ui
@@ -0,0 +1,957 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>TuningControllerTab</class>
+ <widget class="QWidget" name="TuningControllerTab">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1519</width>
+ <height>970</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout">
+ <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="10" column="1">
+ <widget class="QLabel" name="label_21">
+ <property name="text">
+ <string>step 3: repeat steps 1 and 2 until desired performance is achieved</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <spacer name="verticalSpacer_10">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>10</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="4" column="1">
+ <widget class="Line" name="line_10">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <layout class="QGridLayout" name="gridLayout_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 row="1" column="9">
+ <layout class="QVBoxLayout" name="verticalLayout_6">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <spacer name="verticalSpacer_6">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_13">
+ <property name="text">
+ <string>velocity</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_14">
+ <property name="text">
+ <string>[meters/second]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="verticalSpacer_7">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="2">
+ <layout class="QVBoxLayout" name="verticalLayout_4">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>gain</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="9">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_error_D">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <spacer name="verticalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>angle</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>[degrees]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="2">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_gain_P">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="7">
+ <layout class="QVBoxLayout" name="verticalLayout_5">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>gain</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_angle">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="5">
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <spacer name="verticalSpacer_5">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>position</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_10">
+ <property name="text">
+ <string>error</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_11">
+ <property name="text">
+ <string>[meters]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="verticalSpacer_4">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="8">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>x</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_12">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>=</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="7">
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_gain_D">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>x</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="5">
+ <layout class="QHBoxLayout" name="horizontalLayout_5">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_error_P">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="0">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="Line" name="line_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="Line" name="line_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="3">
+ <widget class="Line" name="line_4">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="5">
+ <widget class="Line" name="line_5">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="9">
+ <layout class="QVBoxLayout" name="verticalLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <widget class="Line" name="line_6">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="6">
+ <widget class="Line" name="line_7">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="7">
+ <widget class="Line" name="line_8">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="8">
+ <widget class="Line" name="line_9">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="3">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>x</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_15">
+ <property name="text">
+ <string>+</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="8">
+ <layout class="QHBoxLayout" name="horizontalLayout_12">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_16">
+ <property name="text">
+ <string>x</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="2">
+ <layout class="QHBoxLayout" name="horizontalLayout_14">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QSlider" name="slider_gain_P">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>220</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true"/>
+ </property>
+ <property name="maximum">
+ <number>1000</number>
+ </property>
+ <property name="pageStep">
+ <number>50</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="invertedAppearance">
+ <bool>false</bool>
+ </property>
+ <property name="invertedControls">
+ <bool>false</bool>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksBothSides</enum>
+ </property>
+ <property name="tickInterval">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_15">
+ <property name="leftMargin">
+ <number>12</number>
+ </property>
+ <property name="rightMargin">
+ <number>12</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_22">
+ <property name="text">
+ <string>=</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ <item row="11" column="1">
+ <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="2" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_13">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="topMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_18">
+ <property name="text">
+ <string>setpoint =</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="button_setpoint_toggle">
+ <property name="text">
+ <string>toggle</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="9" column="1">
+ <widget class="QLabel" name="label_20">
+ <property name="text">
+ <string>step 2: click toggle button to test the performance</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_17">
+ <property name="text">
+ <string>Horizontal Controller (Horizontal Regler)</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <spacer name="verticalSpacer_8">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>10</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="8" column="1">
+ <widget class="QLabel" name="label_19">
+ <property name="text">
+ <string>step 1: adjust slider to change the gain</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="Line" name="line_11">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <spacer name="verticalSpacer_9">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>10</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="11" column="2">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_20.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_20.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_20.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_20.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_40.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_40.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_40.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_40.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_60.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_60.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_60.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_60.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_80.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_80.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_80.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_80.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_empty.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_empty.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_empty.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_empty.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_full.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_full.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_full.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_full.png
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_unavailable.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_unavailable.png
new file mode 100644
index 0000000000000000000000000000000000000000..71765cfe18b1d7a915b7f92daf06df3b6558521f
Binary files /dev/null and b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_unavailable.png differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_unknown.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_unknown.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/battery_unknown.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/battery_unknown.png
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/emergency_stop.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/emergency_stop.png
new file mode 100644
index 0000000000000000000000000000000000000000..7643f985406c7be73109dbab7cbf9fd00f6d63b4
Binary files /dev/null and b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/emergency_stop.png differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_disabling.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_disabling.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_disabling.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_disabling.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_enabling.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_enabling.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_enabling.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_enabling.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_flying.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_flying.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_flying.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_flying.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_off.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_off.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_off.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_off.png
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_unavailable.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_unavailable.png
new file mode 100644
index 0000000000000000000000000000000000000000..60f6878b603abc0ba5dd375765ff06ad36a397ab
Binary files /dev/null and b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_unavailable.png differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_unknown.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_unknown.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/flying_state_unknown.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/flying_state_unknown.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/rf_connected.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/rf_connected.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/rf_connected.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/rf_connected.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/rf_connecting.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/rf_connecting.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/rf_connecting.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/rf_connecting.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/rf_disconnected.png b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/rf_disconnected.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/images/rf_disconnected.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/images/rf_disconnected.png
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6d8f5584ef4b07122c32857256b9a206e8b4b47
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
@@ -0,0 +1,211 @@
+// 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:
+// Constants that are used across multiple files
+//
+// ----------------------------------------------------------------------------------
+
+
+// ----------------------------------------------------------------------------------
+// U U
+// U U
+// U U
+// U U
+// UUU
+// ----------------------------------------------------------------------------------
+
+
+// Conversions between degrees and radians
+#define RAD2DEG 180.0/PI
+#define DEG2RAD PI/180.0
+
+// PI
+#define PI 3.141592653589
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// The types, i.e., agent or coordinator
+#define TYPE_INVALID -1
+#define TYPE_COORDINATOR 1
+#define TYPE_AGENT 2
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board the Crazyflie 2.0.
+// The following is a short description about each mode:
+//
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+//
+// CF_COMMAND_TYPE_RATE
+// 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.
+//
+// CF_COMMAND_TYPE_ANGLE
+// 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_MOTORS 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+// Types of controllers being used:
+#define DEFAULT_CONTROLLER 1
+#define DEMO_CONTROLLER 2
+#define STUDENT_CONTROLLER 3
+#define MPC_CONTROLLER 4
+#define REMOTE_CONTROLLER 5
+#define TUNING_CONTROLLER 6
+#define PICKER_CONTROLLER 7
+#define TEMPLATE_CONTROLLER 8
+
+// The constants that "command" changes in the
+// operation state of this agent
+#define CMD_USE_DEFAULT_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_USE_PICKER_CONTROLLER 7
+#define CMD_USE_TEMPLATE_CONTROLLER 8
+
+
+#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
+#define STATE_UNAVAILABLE 5
+
+
+// Commands for CrazyRadio
+#define CMD_RECONNECT 0
+#define CMD_DISCONNECT 1
+
+
+// CrazyRadio states:
+#define CRAZY_RADIO_STATE_CONNECTED 0
+#define CRAZY_RADIO_STATE_CONNECTING 1
+#define CRAZY_RADIO_STATE_DISCONNECTED 2
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
+// ----------------------------------------------------------------------------------
+
+// Battery levels
+#define BATTERY_LEVEL_000 0
+#define BATTERY_LEVEL_010 1
+#define BATTERY_LEVEL_020 2
+#define BATTERY_LEVEL_030 3
+#define BATTERY_LEVEL_040 4
+#define BATTERY_LEVEL_050 5
+#define BATTERY_LEVEL_060 6
+#define BATTERY_LEVEL_070 7
+#define BATTERY_LEVEL_080 8
+#define BATTERY_LEVEL_090 9
+#define BATTERY_LEVEL_100 10
+#define BATTERY_LEVEL_UNAVAILABLE -1
+
+// Battery states
+#define BATTERY_STATE_NORMAL 0
+#define BATTERY_STATE_LOW 1
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// Y Y A M M L
+// Y Y A A MM MM L
+// Y A A M M M L
+// Y AAAAA M M L
+// Y A A M M LLLLL
+// ----------------------------------------------------------------------------------
+
+// For where to load the yaml file from
+#define LOAD_YAML_FROM_AGENT 1
+#define LOAD_YAML_FROM_COORDINATOR 2
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// For standard buttons in the GUI
+#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/main.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/PickerControllerConstants_for_Qt_compile.h
similarity index 65%
rename from pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/main.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/PickerControllerConstants_for_Qt_compile.h
index 27bdde984af32065c2c7c03ddef660376e51004a..556373cec790ce019503e34364e73d1d91c604a2 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/main.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/PickerControllerConstants_for_Qt_compile.h
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
@@ -25,18 +25,32 @@
//
//
// DESCRIPTION:
-// Main program of the Student's GUI
+// Constants that are used across the Picker Controler Service and GUI
//
// ----------------------------------------------------------------------------------
-#include "MainWindow.h"
-#include <QApplication>
-int main(int argc, char *argv[])
-{
- QApplication a(argc, argv);
- MainWindow w(argc, argv);
- w.show();
- return a.exec();
-}
+
+
+// TO IDENITFY THE STATE OF THE PICKER
+
+#define PICKER_STATE_UNKNOWN -1
+#define PICKER_STATE_STANDBY 0
+#define PICKER_STATE_GOTO_START 1
+#define PICKER_STATE_ATTACH 2
+#define PICKER_STATE_LIFT_UP 3
+#define PICKER_STATE_GOTO_END 4
+#define PICKER_STATE_PUT_DOWN 5
+#define PICKER_STATE_SQUAT 6
+#define PICKER_STATE_JUMP 7
+
+
+
+// DEFAULT {x,y,z,yaw,mass}
+
+#define PICKER_DEFAULT_X 0
+#define PICKER_DEFAULT_Y 0
+#define PICKER_DEFAULT_Z 0.4
+#define PICKER_DEFAULT_YAW_DEGREES 0
+#define PICKER_DEFAULT_MASS_GRAMS 30
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h
new file mode 100644
index 0000000000000000000000000000000000000000..ac223f9bddc55cc0fcc21c2631a5f1a978124938
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h
@@ -0,0 +1,234 @@
+// Copyright (C) 2019, 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 bar of the GUI for (dis-)connecting (from)to the Crazyradio
+// and for sending the {take-off,land,motors-off} commands//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef CONNECTSTARTSTOPBAR_H
+#define CONNECTSTARTSTOPBAR_H
+
+#include <string>
+#include <QWidget>
+#include <QMutex>
+
+#include <QTextStream>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/AreaBounds.h"
+#include "dfall_pkg/CrazyflieContext.h"
+#include "dfall_pkg/IntIntService.h"
+#include "dfall_pkg/CMQuery.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+// BATTERY LABEL IMAGE INDEX
+#define BATTERY_LABEL_IMAGE_INDEX_UNVAILABLE -1
+#define BATTERY_LABEL_IMAGE_INDEX_EMPTY 0
+#define BATTERY_LABEL_IMAGE_INDEX_20 1
+#define BATTERY_LABEL_IMAGE_INDEX_40 2
+#define BATTERY_LABEL_IMAGE_INDEX_60 3
+#define BATTERY_LABEL_IMAGE_INDEX_80 4
+#define BATTERY_LABEL_IMAGE_INDEX_FULL 5
+#define BATTERY_LABEL_IMAGE_INDEX_UNKNOWN 6
+
+namespace Ui {
+class ConnectStartStopBar;
+}
+
+class ConnectStartStopBar : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit ConnectStartStopBar(QWidget *parent = 0);
+ ~ConnectStartStopBar();
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+
+
+private:
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+ Ui::ConnectStartStopBar *ui;
+
+ // > For the type of this node,
+ // i.e., an agent or a coordinator
+ int m_type = 0;
+
+ // > For the ID of this node
+ int m_ID = 0;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+
+
+
+ // > For keeping track of the current battery state
+ int m_battery_state;
+ // > For keeping track of which image is currently displayed
+ int m_battery_status_label_image_current_index;
+
+
+
+ // MUTEX FOR HANDLING ACCESS
+ // > For the "rf_status_label" UI element
+ //QMutex m_rf_status_label_mutex;
+ // > For the "battery_voltage_lineEdit" UI element
+ //QMutex m_battery_voltage_lineEdit_mutex;
+ // > For the "battery_status_label" UI element
+ QMutex m_battery_status_label_mutex;
+
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+ // > For making the "enable flight" and "disable flight"
+ // buttons (un-)available
+ void disableFlyingStateButtons();
+ void enableFlyingStateButtons();
+
+ // > For updating the RF Radio status shown in the UI element
+ // of "rf_status_label"
+ void setCrazyRadioStatus(int radio_status);
+
+ // > For updating the battery state
+ void setBatteryState(int new_battery_state);
+ // > For updating the battery voltage shown in the UI elements
+ // of "battery_voltage_lineEdit" and "battery_status_label"
+ void setBatteryVoltageText(float battery_voltage);
+ void setBatteryImageBasedOnLevel(int battery_level);
+
+ // > For updating the "my_flying_state" variable, and the
+ // UI element of "flying_state_label"
+ void setFlyingState(int new_flying_state);
+
+
+
+#ifdef CATKIN_MAKE
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES FOR ROS
+
+
+ // PUBLISHERS AND SUBSRIBERS
+ // > For Crazyradio commands based on button clicks
+ ros::Publisher crazyRadioCommandPublisher;
+ // > For updating the "rf_status_label" picture
+ ros::Subscriber crazyRadioStatusSubscriber;
+
+ // > For updating the current battery voltage
+ ros::Subscriber batteryVoltageSubscriber;
+ // > For updating the current battery state
+ //ros::Subscriber batteryStateSubscriber;
+ // > For updating the current battery level
+ ros::Subscriber batteryLevelSubscriber;
+
+ // > For Flying state commands based on button clicks
+ ros::Publisher flyingStateCommandPublisher;
+ // > For updating the "flying_state_label" picture
+ ros::Subscriber flyingStateSubscriber;
+
+
+ // --------------------------------------------------- //
+ // PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+ // Get the type and ID of this node
+ bool getTypeAndIDParameters();
+ // Fill the head for a message
+ void fillIntMessageHeader( dfall_pkg::IntWithHeader & msg );
+
+ // > For the CrazyRadio status, received on the
+ // "crazyRadioStatusSubscriber"
+ void crazyRadioStatusCallback(const std_msgs::Int32& msg);
+
+ // > For the Battery Voltage, received on the
+ // "batteryVoltageSubscriber"
+ void batteryVoltageCallback(const std_msgs::Float32& msg);
+ // > For the Battery State, receieved on the
+ // "batteryStateSubscriber"
+ void batteryStateChangedCallback(const std_msgs::Int32& msg);
+ // > For the Battery Level, receieved on the
+ // "batteryLevelSubscriber"
+ void batteryLevelCallback(const std_msgs::Int32& msg);
+
+ // > For the Flying State, received on the
+ // "flyingStateSubscriber"
+ void flyingStateChangedCallback(const std_msgs::Int32& msg);
+
+#endif
+
+
+
+
+
+private slots:
+
+ // PRIVATE METHODS FOR BUTTON CALLBACKS
+ // > For the RF Crazyradio connect/disconnect buttons
+ void on_rf_connect_button_clicked();
+ void on_rf_disconnect_button_clicked();
+ // > For the enable, disable, motors-off buttons
+ void on_enable_flying_button_clicked();
+ void on_disable_flying_button_clicked();
+ void on_motors_off_button_clicked();
+
+
+};
+
+#endif // CONNECTSTARTSTOPBAR_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
new file mode 100644
index 0000000000000000000000000000000000000000..eca925ea65b4c4d21022c02a7b2237654aeab443
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
@@ -0,0 +1,186 @@
+// Copyright (C) 2019, 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 tab wdiget that contains and manages the individual tabs used
+// to interface with each separate controller.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef CONTROLLERTABS_H
+#define CONTROLLERTABS_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QVector>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+//#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/AreaBounds.h"
+#include "dfall_pkg/CrazyflieContext.h"
+#include "dfall_pkg/CMQuery.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/IntIntService.h"
+
+// Include the shared definitions
+//#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+//#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+namespace Ui {
+class ControllerTabs;
+}
+
+class ControllerTabs : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit ControllerTabs(QWidget *parent = 0);
+ ~ControllerTabs();
+
+ // PUBLIC METHODS FOR TOGGLING THE VISISBLE CONTROLLERS
+ void showHideController_toggle(QString qstr_label, QWidget * tab_widget_to_toggle);
+ void showHideController_default_changed();
+ void showHideController_student_changed();
+ void showHideController_picker_changed();
+ void showHideController_tuning_changed();
+ void showHideController_template_changed();
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void setObjectNameForDisplayingPoseData( QString object_name );
+
+
+signals:
+ void agentIDsToCoordinateChanged(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void measuredPoseValueChanged(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void poseDataUnavailableSignal();
+
+
+private:
+ Ui::ControllerTabs *ui;
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+ // The object name for which motion capture pose data
+ // will be "emitted" using the "measuredPoseValueChanged"
+ // signal
+ std::string m_object_name_for_emitting_pose_data;
+
+ // Flag for whether pose data should be emitted, this is
+ // to save looking through the data when it is unnecessary
+ bool m_should_search_pose_data_for_object_name = false;
+ QMutex m_should_search_pose_data_for_object_name_mutex;
+
+ // The color for normal and highlighted tabs
+ QColor m_tab_text_colour_normal;
+ QColor m_tab_text_colour_highlight;
+
+
+#ifdef CATKIN_MAKE
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES FOR ROS
+
+ // > For the "context" of this agent
+ dfall_pkg::CrazyflieContext m_context;
+ dfall_pkg::AreaBounds m_area;
+
+ // SUBSRIBER
+ // > For the pose data from a motion capture system
+ ros::Subscriber m_poseDataSubscriber;
+ // > For the controller that is currently operating
+ ros::Subscriber controllerUsedSubscriber;
+
+ // > For changes in the database that defines {agentID,cfID,flying zone} links
+ //ros::Subscriber databaseChangedSubscriber;
+ ros::ServiceClient centralManagerDatabaseService;
+#endif
+
+
+#ifdef CATKIN_MAKE
+ // --------------------------------------------------- //
+ // PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+ // > For the controller currently operating, received on
+ // "controllerUsedSubscriber"
+ void poseDataReceivedCallback(const dfall_pkg::ViconData& viconData);
+
+ void controllerUsedChangedCallback(const std_msgs::Int32& msg);
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+#endif
+
+
+ void setControllerEnabled(int new_controller);
+
+ void setAllTabLabelsToNormalColouring();
+
+ void setTextColourOfTabLabel(QColor color , QWidget * tab_widget);
+
+};
+
+#endif // CONTROLLERTABS_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/rosNodeThread_for_studentGUI.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/coordinator.h
similarity index 51%
rename from pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/rosNodeThread_for_studentGUI.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/coordinator.h
index a7bb587441ceec81540e4b34c5ed4f0cf94d7aea..54635ca289d4091c68490929334ee04a5783e8bb 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/rosNodeThread_for_studentGUI.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/coordinator.h
@@ -1,20 +1,20 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
+// Copyright (C) 2019, 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
@@ -25,69 +25,74 @@
//
//
// DESCRIPTION:
-// Takes care of creating a ros node thread.
+// The coordinator part of the Flying Agent GUI that allows multiple
+// flying agents to be controller from one place, and keeps track
+// of which subset of flying agents to control.
//
// ----------------------------------------------------------------------------------
-#ifndef ___ROSNODETHREAD_FOR_STUDENTGUI_H___
-#define ___ROSNODETHREAD_FOR_STUDENTGUI_H___
-#include <QtCore>
-#include <QThread>
-#include <QStringList>
-#include <stdlib.h>
-#include <QMutex>
-#include <iostream>
-#include "assert.h"
-#include <ros/ros.h>
-#include <ros/network.h>
-#include "d_fall_pps/UnlabeledMarker.h"
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/ViconData.h"
-using namespace d_fall_pps;
+#ifndef COORDINATOR_H
+#define COORDINATOR_H
-typedef ViconData::ConstPtr ptrToMessage;
+#include "coordinatorrow.h"
-Q_DECLARE_METATYPE(ptrToMessage)
+#include <QWidget>
+#include <QVector>
+#include <regex>
+#include <QTextStream>
-class rosNodeThread : public QObject {
- Q_OBJECT
-public:
- explicit rosNodeThread(int argc, char **pArgv, const char * node_name, QObject *parent = 0);
- virtual ~rosNodeThread();
+namespace Ui {
+class Coordinator;
+}
- bool init();
+class Coordinator : public QWidget
+{
+ Q_OBJECT
- // void messageCallback(const ViconData& data);
- void messageCallback(const ptrToMessage& p_msg);
+public:
+ explicit Coordinator(QWidget *parent = 0);
+ ~Coordinator();
- ros::ServiceClient m_read_db_client;
- ros::ServiceClient m_update_db_client;
- ros::ServiceClient m_command_db_client;
+public slots:
+ void setShouldCoordinateThisAgent(int agentID , bool shouldCoordinate);
-signals:
- void newViconData(const ptrToMessage& p_msg);
+signals:
+ void agentIDsToCoordinateChanged(QVector<int> agentIDs , bool shouldCoordinateAll);
-public slots:
- void run();
private:
- int m_Init_argc;
- char** m_pInit_argv;
- const char * m_node_name;
+ QVector<CoordinatorRow*> vector_of_coordinatorRows;
- QThread * m_pThread;
+ QVector<bool> vector_of_shouldCoordinate_perRow;
- ViconData m_vicon_data;
+ QVector<int> vector_of_agentID_perRow;
- ros::Subscriber m_vicon_subscriber;
- // ros::Publisher sim_velocity;
+ int level_of_detail_to_display = 1;
+
+ void remove_all_entries_from_vector_of_coordinatorRows();
+
+ void apply_level_of_detail_to_all_entries(int level);
+
+private slots:
+ void on_refresh_button_clicked();
+
+ void on_toggle_details_button_clicked();
+
+ void on_delete_button_clicked();
+
+ void on_coordinate_all_checkBox_clicked();
+
+ void emit_signal_with_agentIDs_toCoordinate();
+
+private:
+ Ui::Coordinator *ui;
};
-#endif
+#endif // COORDINATOR_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h
similarity index 56%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h
index 8e5ef66452562e6573b2fd28f71c00492ca8ecf9..9174ef6ca334c3daa2e23ef3a472616f8d59a8f4 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
@@ -25,11 +25,16 @@
//
//
// DESCRIPTION:
-// Coordinator Row GUI heder.
+// An individual row in the coordinator part of the Flying Agent
+// GUI. This is essentially a condensed version of the
+// "Connect Start Stop Bar"
//
// ----------------------------------------------------------------------------------
+
+
+
#ifndef COORDINATORROW_H
#define COORDINATORROW_H
@@ -38,71 +43,43 @@
#include <QMutex>
#ifdef CATKIN_MAKE
-#include <std_msgs/Int32.h>
-#include <std_msgs/Float32.h>
-
#include <ros/ros.h>
#include <ros/network.h>
#include <ros/package.h>
-#include "d_fall_pps/AreaBounds.h"
-#include "d_fall_pps/CrazyflieContext.h"
-#include "d_fall_pps/CMQuery.h"
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
-using namespace d_fall_pps;
-#endif
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/AreaBounds.h"
+#include "dfall_pkg/CrazyflieContext.h"
+#include "dfall_pkg/CMQuery.h"
+#include "dfall_pkg/IntIntService.h"
-// TYPES OF CONTROLLER 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
-
-// COMMANDS FOR CRAZYRADIO
-#define CMD_RECONNECT 0
-#define CMD_DISCONNECT 1
-
-// CRAZYRADIO STATES
-#define CONNECTED 0
-#define CONNECTING 1
-#define DISCONNECTED 2
-
-// COMMANDS FOR THE FLYING STATE/CONTROLLER USED
-// The constants that "command" changes in the
-// operation state of this agent. These "commands"
-// are sent from this GUI node to the "PPSClient"
-// node where the command is enacted
-#define CMD_USE_SAFE_CONTROLLER 1
-#define CMD_USE_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
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
// BATTERY LABEL IMAGE INDEX
-#define BATTERY_LABEL_IMAGE_INDEX_EMPTY 0
-#define BATTERY_LABEL_IMAGE_INDEX_20 1
-#define BATTERY_LABEL_IMAGE_INDEX_40 2
-#define BATTERY_LABEL_IMAGE_INDEX_60 3
-#define BATTERY_LABEL_IMAGE_INDEX_80 4
-#define BATTERY_LABEL_IMAGE_INDEX_FULL 5
-#define BATTERY_LABEL_IMAGE_INDEX_UNKNOWN 6
+#define BATTERY_LABEL_IMAGE_INDEX_UNVAILABLE -1
+#define BATTERY_LABEL_IMAGE_INDEX_EMPTY 0
+#define BATTERY_LABEL_IMAGE_INDEX_20 1
+#define BATTERY_LABEL_IMAGE_INDEX_40 2
+#define BATTERY_LABEL_IMAGE_INDEX_60 3
+#define BATTERY_LABEL_IMAGE_INDEX_80 4
+#define BATTERY_LABEL_IMAGE_INDEX_FULL 5
+#define BATTERY_LABEL_IMAGE_INDEX_UNKNOWN 6
namespace Ui {
@@ -120,6 +97,13 @@ public:
// PUBLIC METHODS FOR SETTING PROPERTIES
// > Set the state of the checkbox
void setShouldCoordinate(bool shouldCoordinate);
+ // > Set the level of detail to display
+ void setLevelOfDetailToDisplay(int level);
+
+
+signals:
+ void shouldCoordinateThisAgentValueChanged(int agentID , bool shouldCoordinate);
+
private:
// --------------------------------------------------- //
@@ -127,61 +111,66 @@ private:
Ui::CoordinatorRow *ui;
// > For the ID of which agent this "coordinator row" relates to
- int my_agentID;
+ int m_agentID;
// > For using the agent ID in constructing namespaces
- QString my_agentID_as_string;
+ QString m_agentID_as_string;
+ // > For the name of the allocated Crazyflie
+ QString m_crazyflie_name_as_string;
+
- // > For keeping track of the current RF Crazyradio state
- int my_radio_status;
// > For keeping track of the current battery state
- int my_battery_state;
+ int m_battery_state;
// > For keeping track of which image is currently displayed
- int my_battery_status_label_image_current_index;
- // > For keeping track of the current operating state
- int my_flying_state;
+ int m_battery_status_label_image_current_index;
+
// MUTEX FOR HANDLING ACCESS
// > For the "rf_status_label" UI element
- QMutex my_rf_status_label_mutex;
- // > For the "my_battery_state" variable
- QMutex my_battery_state_mutex;
+ //QMutex m_rf_status_label_mutex;
// > For the "battery_voltage_lineEdit" UI element
- QMutex my_battery_voltage_lineEdit_mutex;
+ //QMutex m_battery_voltage_lineEdit_mutex;
// > For the "battery_status_label" UI element
- QMutex my_battery_status_label_mutex;
- // > For the "my_flying_state" variable
- QMutex my_flying_state_mutex;
+ QMutex m_battery_status_label_mutex;
- // BATTERY VOLTAGE LIMITS (THESE SHOULD BE READ IN AS PARAMTERS)
- // > When in a "standby" type of state
- float battery_voltage_standby_empty;
- float battery_voltage_standby_full;
- // > When in a "flying" type of state
- float battery_voltage_flying_empty;
- float battery_voltage_flying_full;
// --------------------------------------------------- //
// PRIVATE FUNCTIONS
- // > For updating the RF Radio status shown in the UI element of "rf_status_label"
+ // > For making the "enable flight" and "disable flight" buttons
+ // (un-)available
+ void disableFlyingStateButtons();
+ void enableFlyingStateButtons();
+
+ // > For updating the RF Radio status shown in the UI element
+ // of "rf_status_label"
void setCrazyRadioStatus(int radio_status);
+
// > For updating the battery state
void setBatteryState(int new_battery_state);
- // > For updating the battery voltage shown in the UI elements of "battery_voltage_lineEdit" and "battery_status_label"
- void setBatteryVoltageTextAndImage(float battery_voltage);
+ // > For updating the battery voltage shown in the UI elements
+ // of "battery_voltage_lineEdit" and "battery_status_label"
void setBatteryVoltageText(float battery_voltage);
- void setBatteryVoltageImage(float battery_voltage);
- // > For converting a voltage to a percentage, depending on the current "my_flying_state" value
- float fromVoltageToPercent(float voltage);
- // > For making the "enable flight" and "disable flight" buttons (un-)available
- void disableFlyingStateButtons();
- void enableFlyingStateButtons();
- // > For updating the "my_flying_state" variable, and the UI element of "flying_state_label"
+ void setBatteryImageBasedOnLevel(int battery_level);
+
+ // > For updating the "my_flying_state" variable, and the
+ // UI element of "flying_state_label"
void setFlyingState(int new_flying_state);
- // > For loading the "context" for this agent, i.e., the {agentID,cfID,flying zone} tuple
+
+ // > For loading the "context" for this agent,
+ // i.e., the {agentID,cfID,flying zone} tuple
void loadCrazyflieContext();
- // > For updating the text in the UI element of "controller_enabled_label"
+
+ // > For requesting the current flying state
+ // i.e., using the service advertised by the Flying Agent Client
+ void getCurrentFlyingState();
+
+ // > For requesting the current state of the Crazy Radio
+ // i.e., using the service advertised by the Flying Agent Client
+ void getCurrentCrazyRadioState();
+
+ // > For updating the text in the UI element of
+ // "controller_enabled_label"
void setControllerEnabled(int new_controller);
@@ -190,49 +179,69 @@ private:
// --------------------------------------------------- //
// PRIVATE VARIABLES FOR ROS
- // > For running this is a ROS node thread
- //rosNodeThread* myrosNodeThread;
-
- // > For the namespace of this node
- std::string my_ros_namespace;
-
// > For the "context" of this agent
- CrazyflieContext my_context;
+ dfall_pkg::CrazyflieContext my_context;
// PUBLISHERS AND SUBSRIBERS
// > For Crazyradio commands based on button clicks
ros::Publisher crazyRadioCommandPublisher;
// > For updating the "rf_status_label" picture
ros::Subscriber crazyRadioStatusSubscriber;
+
// > For updating the current battery voltage
ros::Subscriber batteryVoltageSubscriber;
// > For updating the current battery state
- ros::Subscriber batteryStateSubscriber;
+ //ros::Subscriber batteryStateSubscriber;
+ // > For updating the current battery level
+ ros::Subscriber batteryLevelSubscriber;
+
// > For Flying state commands based on button clicks
ros::Publisher flyingStateCommandPublisher;
// > For updating the "flying_state_label" picture
ros::Subscriber flyingStateSubscriber;
+
// > For changes in the database that defines {agentID,cfID,flying zone} links
ros::Subscriber databaseChangedSubscriber;
ros::ServiceClient centralManagerDatabaseService;
// > For updating the controller that is currently operating
ros::Subscriber controllerUsedSubscriber;
+ // > For requesting the current flying state,
+ // this is used only for initialising the icon
+ ros::ServiceClient getCurrentFlyingStateService;
+
+ // > For requesting the current state of the Crazy Radio,
+ // this is used only for initialising the icon
+ ros::ServiceClient getCurrentCrazyRadioStateService;
+
// --------------------------------------------------- //
// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
- // > For the CrazyRadio status, received on the "crazyRadioStatusSubscriber"
+ // > For the CrazyRadio status, received on the
+ // "crazyRadioStatusSubscriber"
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
- // > For the Battery Voltage, received on the "batteryVoltageSubscriber"
+
+ // > For the Battery Voltage, received on the
+ // "batteryVoltageSubscriber"
void batteryVoltageCallback(const std_msgs::Float32& msg);
- // > For the Battery State, receieved on the "batteryStateSubscriber"
+ // > For the Battery State, receieved on the
+ // "batteryStateSubscriber"
void batteryStateChangedCallback(const std_msgs::Int32& msg);
- // > For the Flying State, received on the "flyingStateSubscriber"
+ // > For the Battery Level, receieved on the
+ // "batteryLevelSubscriber"
+ void batteryLevelCallback(const std_msgs::Int32& msg);
+
+ // > For the Flying State, received on the
+ // "flyingStateSubscriber"
void flyingStateChangedCallback(const std_msgs::Int32& msg);
- // > For the notification that the database was changes, received on the "DatabaseChangedSubscriber"
+
+ // > For the notification that the database was changes,
+ // received on the "DatabaseChangedSubscriber"
void databaseChangedCallback(const std_msgs::Int32& msg);
- // > For the controller currently operating, received on "controllerUsedSubscriber"
+
+ // > For the controller currently operating, received on
+ // "controllerUsedSubscriber"
void controllerUsedChangedCallback(const std_msgs::Int32& msg);
@@ -251,6 +260,9 @@ private slots:
void on_disable_flying_button_clicked();
void on_motors_off_button_clicked();
+ // > For the "should coordinate" checkbox
+ void on_shouldCoordinate_checkBox_clicked();
+
};
#endif // COORDINATORROW_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
new file mode 100644
index 0000000000000000000000000000000000000000..64f315c4f1710841b4c8bdb502a913f57fad39ab
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
@@ -0,0 +1,172 @@
+// Copyright (C) 2019, 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 GUI for the Default Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef DEFAULTCONTROLLERTAB_H
+#define DEFAULTCONTROLLERTAB_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QVector>
+#include <QTextStream>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+//#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+//#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+#include "nodes/DefaultControllerConstants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+
+namespace Ui {
+class DefaultControllerTab;
+}
+
+class DefaultControllerTab : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit DefaultControllerTab(QWidget *parent = 0);
+ ~DefaultControllerTab();
+
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void poseDataUnavailableSlot();
+
+
+
+private slots:
+ void on_lineEdit_setpoint_new_x_returnPressed();
+ void on_lineEdit_setpoint_new_y_returnPressed();
+ void on_lineEdit_setpoint_new_z_returnPressed();
+ void on_lineEdit_setpoint_new_yaw_returnPressed();
+
+ void on_set_setpoint_button_clicked();
+
+ void on_default_setpoint_button_clicked();
+
+ void on_x_increment_plus_button_clicked();
+ void on_x_increment_minus_button_clicked();
+ void on_y_increment_plus_button_clicked();
+ void on_y_increment_minus_button_clicked();
+ void on_z_increment_plus_button_clicked();
+ void on_z_increment_minus_button_clicked();
+ void on_yaw_increment_plus_button_clicked();
+ void on_yaw_increment_minus_button_clicked();
+
+
+
+private:
+ Ui::DefaultControllerTab *ui;
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+ // Mutex for the current state label
+ QMutex m_label_current_state_mutex;
+
+
+
+#ifdef CATKIN_MAKE
+ // PUBLISHER
+ // > For requesting the setpoint to be changed
+ ros::Publisher requestSetpointChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
+#endif
+
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+
+#ifdef CATKIN_MAKE
+ // For receiving message that the setpoint was changed
+ void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+
+ // Fill the header for a message
+ void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+#endif
+
+ void publishSetpoint(float x, float y, float z, float yaw);
+
+};
+
+#endif // DEFAULTCONTROLLERTAB_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ffd3ff9f83268b23da78a423237137f1a81a7c9
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
@@ -0,0 +1,163 @@
+// Copyright (C) 2019, 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 GUI bar for selecting which controller is active, and
+// for request that paramters are loaded from the respective
+// YAML files.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef ENABLECONTROLLERLOADYAMLBAR_H
+#define ENABLECONTROLLERLOADYAMLBAR_H
+
+#include <QWidget>
+#include <QMutex>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/StringWithHeader.h"
+
+// Include the DFALL service types
+// #include "dfall_pkg/AreaBounds.h"
+// #include "dfall_pkg/CrazyflieContext.h"
+// #include "dfall_pkg/CMQuery.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+
+
+
+namespace Ui {
+class EnableControllerLoadYamlBar;
+}
+
+class EnableControllerLoadYamlBar : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit EnableControllerLoadYamlBar(QWidget *parent = 0);
+ ~EnableControllerLoadYamlBar();
+
+ // PUBLIC METHODS FOR TOGGLING THE VISISBLE CONTROLLERS
+ void showHideController_default_changed();
+ void showHideController_student_changed();
+ void showHideController_picker_changed();
+ void showHideController_tuning_changed();
+ void showHideController_template_changed();
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+
+
+private slots:
+
+ // ENABLE CONTROLLER BUTTONS ON-CLICK CALLBACK
+ void on_enable_default_button_clicked();
+ void on_enable_student_button_clicked();
+ void on_enable_picker_button_clicked();
+ void on_enable_tuning_button_clicked();
+ void on_enable_template_button_clicked();
+
+ // LOAD YAML BUTTONS ON-CLICK CALLBACK
+ void on_load_yaml_default_button_clicked();
+ void on_load_yaml_student_button_clicked();
+ void on_load_yaml_picker_button_clicked();
+ void on_load_yaml_tuning_button_clicked();
+ void on_load_yaml_template_button_clicked();
+
+
+private:
+ Ui::EnableControllerLoadYamlBar *ui;
+
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+
+#ifdef CATKIN_MAKE
+ // PUBLISHERS AND SUBSRIBERS
+ // > For {take-off,land,motors-off} and controller selection
+ ros::Publisher commandPublisher;
+ // > For requesting the loading of yaml files
+ ros::Publisher m_requestLoadYamlFilenamePublisher;
+
+#endif
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+#ifdef CATKIN_MAKE
+ // Fill the header for a message
+ void fillIntMessageHeader( dfall_pkg::IntWithHeader & msg );
+ void fillStringMessageHeader( dfall_pkg::StringWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+#endif
+
+
+};
+
+#endif // ENABLECONTROLLERLOADYAMLBAR_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
new file mode 100644
index 0000000000000000000000000000000000000000..916869bd8ed40c3807059ec66cfe184db17b2f5f
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
@@ -0,0 +1,138 @@
+// Copyright (C) 2019, 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 Flying Agent GUI main window.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef MAINWINDOW_FLYINGAGENTGUI_H
+#define MAINWINDOW_FLYINGAGENTGUI_H
+
+#include <QMainWindow>
+#include <QShortcut>
+#include <QMutex>
+
+#include <QTextStream>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+#include "rosNodeThread_for_flyingAgentGUI.h"
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/StringWithHeader.h"
+
+#include "nodes/Constants.h"
+
+// Namespacing the package
+using namespace dfall_pkg;
+//using namespace std;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+namespace Ui {
+class MainWindow;
+}
+
+class MainWindow : public QMainWindow
+{
+ Q_OBJECT
+
+public:
+ explicit MainWindow(int argc, char **argv, QWidget *parent = 0);
+ ~MainWindow();
+
+
+
+private:
+ Ui::MainWindow *ui;
+
+ QShortcut* m_close_GUI_shortcut;
+
+ // > For the type of this node,
+ // i.e., an agent or a coordinator
+ int m_type = 0;
+
+ // > For the ID of this node
+ int m_ID = 0;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+
+#ifdef CATKIN_MAKE
+ rosNodeThread* m_rosNodeThread;
+
+ // The namespace into which this parameter service loads yaml parameters
+ std::string m_parameter_service_namespace;
+
+ ros::Publisher m_requestLoadYamlFilenamePublisher;
+#endif
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+#ifdef CATKIN_MAKE
+ bool getTypeAndIDParameters();
+#endif
+
+
+
+
+private slots:
+ // PRIVATE METHODS FOR MENU ITEM CALLBACKS
+ void on_actionShowHide_Coordinator_triggered();
+ void on_action_LoadYAML_BatteryMonitor_triggered();
+ void on_action_LoadYAML_FlyingAgentClientConfig_triggered();
+
+ // FOR THE CONTROLLERS MENU
+ void on_action_showHideController_default_changed();
+ void on_action_showHideController_student_changed();
+ void on_action_showHideController_picker_changed();
+ void on_action_showHideController_tuning_changed();
+ void on_action_showHideController_template_changed();
+
+};
+
+#endif // MAINWINDOW_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
new file mode 100644
index 0000000000000000000000000000000000000000..52b79e13875c744e4351bcca11989c6a293cc87a
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
@@ -0,0 +1,334 @@
+// Copyright (C) 2019, 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 GUI for the Picker Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef PICKERCONTROLLERTAB_H
+#define PICKERCONTROLLERTAB_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QTimer>
+#include <QVector>
+#include <QTextStream>
+
+#define DECIMAL_PLACES_POSITION 2
+#define DECIMAL_PLACES_ANGLE_DEGREES 1
+#define DECIMAL_PLACES_MASS_GRAMS 1
+
+#define DEFAULT_INCREMENT_POSITION_XY 0.01
+#define DEFAULT_INCREMENT_POSITION_Z 0.01
+#define DEFAULT_INCREMENT_ANGLE_DEGREES 5
+#define DEFAULT_INCREMENT_MASS_GRAMS 0.5
+
+// #define PICKER_STATE_UNKNOWN -1
+// #define PICKER_STATE_STANDBY 0
+// #define PICKER_STATE_GOTO_START 1
+// #define PICKER_STATE_ATTACH 2
+// #define PICKER_STATE_LIFT_UP 3
+// #define PICKER_STATE_GOTO_END 4
+// #define PICKER_STATE_PUT_DOWN 5
+// #define PICKER_STATE_SQUAT 6
+// #define PICKER_STATE_JUMP 7
+
+// #define PICKER_DEFAULT_X 0
+// #define PICKER_DEFAULT_Y 0
+// #define PICKER_DEFAULT_Z 0.4
+// #define PICKER_DEFAULT_YAW_DEGREES 0
+// #define PICKER_DEFAULT_MASS_GRAMS 30
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+//#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+//#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+//#include "dfall_pkg/CustomButtonWithHeader.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+#include "nodes/PickerControllerConstants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+#include "include/PickerControllerConstants_for_Qt_compile.h"
+
+#endif
+
+
+
+namespace Ui {
+class PickerControllerTab;
+}
+
+class PickerControllerTab : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit PickerControllerTab(QWidget *parent = 0);
+ ~PickerControllerTab();
+
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void poseDataUnavailableSlot();
+
+
+
+private slots:
+
+ // FOR ALL THE GUI BUTTONS AND FIELDS
+
+ void on_button_goto_start_clicked();
+
+ void on_button_attach_clicked();
+
+ void on_button_lift_up_clicked();
+
+ void on_button_goto_end_clicked();
+
+ void on_button_put_down_clicked();
+
+ void on_button_squat_clicked();
+
+ void on_button_jump_clicked();
+
+ void on_button_standby_clicked();
+
+ void on_checkbox_goto_start_clicked();
+
+ void on_checkbox_attach_clicked();
+
+ void on_checkbox_lift_up_clicked();
+
+ void on_checkbox_goto_end_clicked();
+
+ void on_checkbox_put_down_clicked();
+
+ void on_checkbox_squat_clicked();
+
+ void on_checkbox_jump_clicked();
+
+ void on_checkbox_standby_clicked();
+
+ void on_button_goto_start_inc_minus_x_clicked();
+
+ void on_button_goto_start_inc_plus_x_clicked();
+
+ void on_button_goto_start_inc_minus_y_clicked();
+
+ void on_button_goto_start_inc_plus_y_clicked();
+
+ void on_button_goto_start_inc_minus_z_clicked();
+
+ void on_button_goto_start_inc_plus_z_clicked();
+
+ void on_button_goto_start_inc_minus_yaw_clicked();
+
+ void on_button_goto_start_inc_plus_yaw_clicked();
+
+ void on_button_attach_inc_minus_z_clicked();
+
+ void on_button_attach_inc_plus_z_clicked();
+
+ void on_button_lift_up_inc_minus_z_clicked();
+
+ void on_button_lift_up_inc_plus_z_clicked();
+
+ void on_button_lift_up_inc_minus_mass_clicked();
+
+ void on_button_lift_up_inc_plus_mass_clicked();
+
+ void on_button_goto_end_inc_minus_x_clicked();
+
+ void on_button_goto_end_inc_plus_x_clicked();
+
+ void on_button_goto_end_inc_minus_y_clicked();
+
+ void on_button_goto_end_inc_plus_y_clicked();
+
+ void on_button_goto_end_inc_minus_yaw_clicked();
+
+ void on_button_goto_end_inc_plus_yaw_clicked();
+
+ void on_button_put_down_inc_minus_z_clicked();
+
+ void on_button_put_down_inc_plus_z_clicked();
+
+ void on_button_squat_inc_minus_z_clicked();
+
+ void on_button_squat_inc_plus_z_clicked();
+
+ void on_button_jump_inc_minus_z_clicked();
+
+ void on_button_jump_inc_plus_z_clicked();
+
+ void on_button_stanby_inc_minus_x_clicked();
+
+ void on_button_stanby_inc_plus_x_clicked();
+
+ void on_button_stanby_inc_minus_y_clicked();
+
+ void on_button_stanby_inc_plus_y_clicked();
+
+ void on_button_stanby_inc_minus_z_clicked();
+
+ void on_button_stanby_inc_plus_z_clicked();
+
+ void on_button_stanby_inc_minus_yaw_clicked();
+
+ void on_button_stanby_inc_plus_yaw_clicked();
+
+ void on_button_stanby_inc_minus_mass_clicked();
+
+ void on_button_stanby_inc_plus_mass_clicked();
+
+ void on_lineEdit_goto_start_x_editingFinished();
+
+ void on_lineEdit_goto_start_y_editingFinished();
+
+ void on_lineEdit_goto_start_z_editingFinished();
+
+ void on_lineEdit_goto_start_yaw_editingFinished();
+
+ void on_lineEdit_attach_z_editingFinished();
+
+ void on_lineEdit_lift_up_z_editingFinished();
+
+ void on_lineEdit_lift_up_mass_editingFinished();
+
+ void on_lineEdit_goto_end_x_editingFinished();
+
+ void on_lineEdit_goto_end_y_editingFinished();
+
+ void on_lineEdit_goto_end_yaw_editingFinished();
+
+ void on_lineEdit_put_down_z_editingFinished();
+
+ void on_lineEdit_squat_z_editingFinished();
+
+ void on_lineEdit_jump_z_editingFinished();
+
+ void on_lineEdit_standby_x_editingFinished();
+
+ void on_lineEdit_standby_y_editingFinished();
+
+ void on_lineEdit_standby_z_editingFinished();
+
+ void on_lineEdit_standby_yaw_editingFinished();
+
+ void on_lineEdit_standby_mass_editingFinished();
+
+ void on_lineEdit_increment_x_editingFinished();
+
+ void on_lineEdit_increment_y_editingFinished();
+
+ void on_lineEdit_increment_z_editingFinished();
+
+ void on_lineEdit_increment_yaw_editingFinished();
+
+ void on_lineEdit_increment_mass_editingFinished();
+
+
+
+private:
+ Ui::PickerControllerTab *ui;
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+ // THE CURRENT STATE OF THE PICKER
+ int m_current_picker_state = PICKER_STATE_UNKNOWN;
+ QMutex m_current_picker_state_mutex;
+
+#ifdef CATKIN_MAKE
+ // PUBLISHER
+ // > For requesting the setpoint to be changed
+ ros::Publisher requestSetpointChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
+#endif
+
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+#ifdef CATKIN_MAKE
+ // For receiving message that the setpoint was changed
+ void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+
+ // Fill the header for a message
+ void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+#endif
+
+ void publish_setpoint_if_current_state_matches(QVector<int> state_to_match);
+
+ void publish_request_setpoint_change_for_state(int state_to_publish);
+};
+
+#endif // PICKERCONTROLLERTAB_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h
similarity index 89%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h
index 0d881f18a2139b6691ee6803c55a7775145615d9..79c447e84f9131164b1faaef335d1d89c06e7ea4 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
@@ -30,6 +30,9 @@
// ----------------------------------------------------------------------------------
+
+
+
#ifndef ___ROSNODETHREAD_FOR_FLYINGAGENTGUI_H___
#define ___ROSNODETHREAD_FOR_FLYINGAGENTGUI_H___
@@ -44,15 +47,15 @@
#include <ros/ros.h>
#include <ros/network.h>
-#include "d_fall_pps/CMRead.h"
-#include "d_fall_pps/CMUpdate.h"
-#include "d_fall_pps/CMCommand.h"
+#include "dfall_pkg/CMRead.h"
+#include "dfall_pkg/CMUpdate.h"
+#include "dfall_pkg/CMCommand.h"
-//#include "d_fall_pps/UnlabeledMarker.h"
-//#include "d_fall_pps/CrazyflieData.h"
-//#include "d_fall_pps/ViconData.h"
+//#include "dfall_pkg/UnlabeledMarker.h"
+//#include "dfall_pkg/CrazyflieData.h"
+//#include "dfall_pkg/ViconData.h"
-using namespace d_fall_pps;
+using namespace dfall_pkg;
//typedef ViconData::ConstPtr ptrToMessage;
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/safecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/safecontrollertab.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/safecontrollertab.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/safecontrollertab.h
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
new file mode 100644
index 0000000000000000000000000000000000000000..be7ed5f428526feac37a3651024fd53c41c22a30
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
@@ -0,0 +1,186 @@
+// Copyright (C) 2019, 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 GUI for the Student Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef STUDENTCONTROLLERTAB_H
+#define STUDENTCONTROLLERTAB_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QVector>
+#include <QLineEdit>
+#include <QTextStream>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+//#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+//#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+
+namespace Ui {
+class StudentControllerTab;
+}
+
+class StudentControllerTab : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit StudentControllerTab(QWidget *parent = 0);
+ ~StudentControllerTab();
+
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void poseDataUnavailableSlot();
+
+
+
+private slots:
+ void on_lineEdit_setpoint_new_x_returnPressed();
+ void on_lineEdit_setpoint_new_y_returnPressed();
+ void on_lineEdit_setpoint_new_z_returnPressed();
+ void on_lineEdit_setpoint_new_yaw_returnPressed();
+
+ void on_set_setpoint_button_clicked();
+
+ void on_default_setpoint_button_clicked();
+
+ void on_x_increment_plus_button_clicked();
+ void on_x_increment_minus_button_clicked();
+ void on_y_increment_plus_button_clicked();
+ void on_y_increment_minus_button_clicked();
+ void on_z_increment_plus_button_clicked();
+ void on_z_increment_minus_button_clicked();
+ void on_yaw_increment_plus_button_clicked();
+ void on_yaw_increment_minus_button_clicked();
+
+ void on_custom_button_1_clicked();
+ void on_custom_button_2_clicked();
+ void on_custom_button_3_clicked();
+ void on_custom_button_4_clicked();
+ void on_custom_button_5_clicked();
+
+
+
+private:
+ Ui::StudentControllerTab *ui;
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+
+
+#ifdef CATKIN_MAKE
+ // PUBLISHER
+ // > For requesting the setpoint to be changed
+ ros::Publisher requestSetpointChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
+
+ // PUBLISHER
+ // > For notifying that a custom button is pressed
+ ros::Publisher customButtonPublisher;
+#endif
+
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+
+#ifdef CATKIN_MAKE
+ // For receiving message that the setpoint was changed
+ void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+
+ // Publish a message when a custom button is pressed
+ void publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer);
+
+ // Fill the header for a message
+ void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
+ void fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+#endif
+
+ void publishSetpoint(float x, float y, float z, float yaw_degrees);
+
+ void increment_setpoint_by(float x_inc, float y_inc, float z_inc, float yaw_inc_degrees);
+
+};
+
+#endif // STUDENTCONTROLLERTAB_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/templatecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/templatecontrollertab.h
new file mode 100644
index 0000000000000000000000000000000000000000..0f9377bb2fa435ca5b98dd792bbde4b863b6fbc2
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/templatecontrollertab.h
@@ -0,0 +1,176 @@
+// Copyright (C) 2019, 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 GUI for a Template Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef TEMPLATECONTROLLERTAB_H
+#define TEMPLATECONTROLLERTAB_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QVector>
+#include <QLineEdit>
+#include <QTextStream>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+//#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+//#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+
+
+
+namespace Ui {
+class TemplateControllerTab;
+}
+
+class TemplateControllerTab : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit TemplateControllerTab(QWidget *parent = 0);
+ ~TemplateControllerTab();
+
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void poseDataUnavailableSlot();
+
+
+
+private slots:
+ void on_lineEdit_setpoint_new_x_returnPressed();
+ void on_lineEdit_setpoint_new_y_returnPressed();
+ void on_lineEdit_setpoint_new_z_returnPressed();
+ void on_lineEdit_setpoint_new_yaw_returnPressed();
+
+ void on_set_setpoint_button_clicked();
+
+ void on_default_setpoint_button_clicked();
+
+ void on_custom_button_1_clicked();
+ void on_custom_button_2_clicked();
+ void on_custom_button_3_clicked();
+
+
+
+
+
+private:
+ Ui::TemplateControllerTab *ui;
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+
+
+#ifdef CATKIN_MAKE
+ // PUBLISHER
+ // > For requesting the setpoint to be changed
+ ros::Publisher requestSetpointChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
+
+ // PUBLISHER
+ // > For notifying that a custom button is pressed
+ ros::Publisher customButtonPublisher;
+#endif
+
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+#ifdef CATKIN_MAKE
+ // For receiving message that the setpoint was changed
+ void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+
+ // Publish a message when a custom button is pressed
+ void publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer);
+
+ // Fill the header for a message
+ void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
+ void fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+#endif
+
+ void publishSetpoint(float x, float y, float z, float yaw_degrees);
+
+};
+
+#endif // TEMPLATECONTROLLERTAB_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/topbanner.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/topbanner.h
new file mode 100644
index 0000000000000000000000000000000000000000..f33161efaaaa8b5cb8a38b72fea0ad185794b42b
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/topbanner.h
@@ -0,0 +1,166 @@
+// Copyright (C) 2019, 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 title displayed at the top of the Flying Agent GUI
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef TOPBANNER_H
+#define TOPBANNER_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QTimer>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/AreaBounds.h"
+#include "dfall_pkg/CrazyflieContext.h"
+#include "dfall_pkg/CMQuery.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#endif
+
+namespace Ui {
+class TopBanner;
+}
+
+class TopBanner : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit TopBanner(QWidget *parent = 0);
+ ~TopBanner();
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+
+
+signals:
+ void objectNameForDisplayingPoseDataValueChanged( QString object_name );
+
+
+private:
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+ Ui::TopBanner *ui;
+
+ // > For the type of this node,
+ // i.e., an agent or a coordinator
+ int m_type = 0;
+
+ // > For the ID of this node
+ int m_ID = 0;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+ // The namespace into which node operates
+ std::string m_base_namespace;
+
+ // The object name for which motion capture pose data
+ // will be "emitted" using the "measuredPoseValueChanged"
+ // signal
+ QString m_object_name_for_emitting_pose_data;
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+ // > For loading the "context" for this agent,
+ // i.e., the {agentID,cfID,flying zone} tuple
+ void loadCrazyflieContext(int ID_to_request_from_database , int emit_after_milliseconds);
+
+
+
+
+
+#ifdef CATKIN_MAKE
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES FOR ROS
+
+ // > For the "context" of this agent
+ dfall_pkg::CrazyflieContext my_context;
+
+ // PUBLISHERS AND SUBSRIBERS
+
+ // > For the emergency stop button
+ ros::Publisher emergencyStopPublisher;
+
+ // > For changes in the database that defines {agentID,cfID,flying zone} links
+ ros::Subscriber databaseChangedSubscriber;
+ ros::ServiceClient centralManagerDatabaseService;
+
+
+ // --------------------------------------------------- //
+ // PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+ // Get the type and ID of this node
+ bool getTypeAndIDParameters();
+
+ // > For the notification that the database was changes,
+ // received on the "DatabaseChangedSubscriber"
+ void databaseChangedCallback(const std_msgs::Int32& msg);
+#endif
+
+
+private slots:
+
+ // PRIVATE METHODS FOR BUTTON CALLBACKS
+ // > For the emergency stop button
+ void on_emergency_stop_button_clicked();
+
+ // > For emitting a signal with the object name after
+ // some small delay
+ void emitObjectNameForDisplayingPoseDataValueChanged();
+
+};
+
+#endif // TOPBANNER_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf8ecf0525abc397d37426452aef8d3c4a90bba1
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h
@@ -0,0 +1,203 @@
+// Copyright (C) 2019, 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 GUI for the Tuning Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef TUNINGCONTROLLERTAB_H
+#define TUNINGCONTROLLERTAB_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QVector>
+#include <QLineEdit>
+#include <QTextStream>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+//#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+//#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/FloatWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+#define P_GAIN_MIN_GUI 0.05
+#define P_GAIN_MAX_GUI 1.60
+#define P_TO_D_GAIN_RATIO_GUI 0.6
+
+#define DECIMAL_PLACES_POSITION_MEASURED 3
+#define DECIMAL_PLACES_ANGLE_DEGREES 1
+#define DECIMAL_PLACES_VELOCITY 2
+#define DECIMAL_PLACES_GAIN 2
+
+#define DECIMAL_PLACES_SETPOINT 2
+
+#define SETPOINT_X_MINUS -0.25
+#define SETPOINT_X_PLUS 0.25
+
+#define SETPOINT_Y 0.0
+#define SETPOINT_Z 0.4
+#define SETPOINT_YAW_DEGREES 0.0
+
+#define MEASUREMENT_FRQUENCY 200.0
+
+
+
+namespace Ui {
+class TuningControllerTab;
+}
+
+class TuningControllerTab : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit TuningControllerTab(QWidget *parent = 0);
+ ~TuningControllerTab();
+
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void poseDataUnavailableSlot();
+
+
+
+private slots:
+ void on_button_setpoint_toggle_clicked();
+
+ void on_lineEdit_setpoint_editingFinished();
+
+ void on_slider_gain_P_valueChanged(int value);
+
+
+
+private:
+ Ui::TuningControllerTab *ui;
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+
+
+ float m_gain_P = 0.0;
+ float m_gain_D = 0.0;
+
+ float m_current_setpoint = 0.0;
+
+ QMutex m_gain_setpoint_mutex;
+
+ float m_x_previous = 0.0;
+
+
+
+ #ifdef CATKIN_MAKE
+ // PUBLISHER
+ // > For requesting the setpoint to be changed
+ ros::Publisher requestSetpointChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
+
+ // PUBLISHER
+ // > For notifying the P gain is changed
+ ros::Publisher requestNewGainChangePublisher;
+
+ // PUBLISHER
+ // > For notifying that a custom button is pressed
+ //ros::Publisher customButtonPublisher;
+ #endif
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+
+ #ifdef CATKIN_MAKE
+ // For receiving message that the setpoint was changed
+ void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+
+ // Publish a message when a custom button is pressed
+ //void publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer);
+
+ // Fill the header for a message
+ void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
+ void fillFloatMessageHeader( dfall_pkg::FloatWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+ #endif
+
+ void publishSetpoint(float x, float y, float z, float yaw);
+
+ void publishGain(float new_gain);
+
+};
+
+#endif // TUNINGCONTROLLERTAB_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c82fc96bf01161a5c59df58bf1edf0c5a8d7770b
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp
@@ -0,0 +1,845 @@
+// Copyright (C) 2019, 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 bar of the GUI for (dis-)connecting (from)to the Crazyradio
+// and for sending the {take-off,land,motors-off} commands
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "connectstartstopbar.h"
+#include "ui_connectstartstopbar.h"
+
+
+
+
+
+ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::ConnectStartStopBar)
+{
+ ui->setupUi(this);
+
+
+#ifdef CATKIN_MAKE
+ // Get the namespace of this node
+ std::string base_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CONNECT START STOP GUI BAR] ros::this_node::getNamespace() = " << base_namespace);
+
+
+ // Get the type and ID of this parameter service
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the TYPE and ID are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[CONNECT START STOP GUI BAR] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+#else
+ // Default as a coordinator when compiling with QtCreator
+ m_type = TYPE_COORDINATOR;
+ m_ID = 1;
+#endif
+
+ // SET THE INITIAL VALUE OF THE PRIVATE VARIABLES FOR THIS CLASS
+ // > For keeping track of the current battery state
+ m_battery_state = BATTERY_STATE_NORMAL;
+ // > For keeping track of which image is currently displayed
+ m_battery_status_label_image_current_index = -999;
+
+
+ // SET THE STARTING RADIO STATUS TO BE: DISCONNECTED
+ setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+
+ // SET THE STARTING BATTERY VOLTAGE TO BE: UNKNOWN
+ setBatteryVoltageText(-1.0f);
+
+ // SET THE STARTING BATTERY LEVEL TO BE: UNAVAILABLE
+ setBatteryImageBasedOnLevel(BATTERY_LEVEL_UNAVAILABLE);
+
+ // SET THE STARTING FLYING STATE STATUS TO BE: MOTORS OFF
+ setFlyingState(STATE_UNAVAILABLE);
+
+ // ENSURE THE F:YING STATE BUTTONS ARE AVAILABLE FOR A COORDINATOR
+ if (m_type == TYPE_COORDINATOR)
+ {
+ enableFlyingStateButtons();
+ }
+
+
+
+#ifdef CATKIN_MAKE
+ // CREATE A NODE HANDLE TO THE BASE NAMESPACE
+ ros::NodeHandle base_nodeHandle(base_namespace);
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ //ros::NodeHandle dfall_root_nodeHandle("/dfall");
+
+ // SUBSCRIBERS AND PUBLISHERS:
+
+ // > For Crazyradio commands based on button clicks
+ crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/crazyRadioCommand", 1);
+
+ // > For Flying state commands based on button clicks
+ flyingStateCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1);
+
+ if (m_type == TYPE_AGENT)
+ {
+ // > For updating the "rf_status_label" picture
+ crazyRadioStatusSubscriber = base_nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &ConnectStartStopBar::crazyRadioStatusCallback, this);
+
+ // > For updating the current battery voltage
+ batteryVoltageSubscriber = base_nodeHandle.subscribe("BatteryMonitor/FilteredVoltage", 1, &ConnectStartStopBar::batteryVoltageCallback, this);
+
+ // > For updating the current battery state
+ //batteryStateSubscriber = base_nodeHandle.subscribe("BatteryMonitor/ChangedStateTo", 1, &ConnectStartStopBar::batteryStateChangedCallback, this);
+
+ // > For updating the current battery level
+ batteryLevelSubscriber = base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &ConnectStartStopBar::batteryLevelCallback, this);
+
+ // > For updating the "flying_state_label" picture
+ flyingStateSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
+ }
+#endif
+
+ // FURTHER INITILIASATIONS NEED TO OCCUR AFTER THE ROS RELATED
+ // INITIALISATIONS ARE COMPLETE
+ //if (m_type == TYPE_AGENT)
+ //{
+ // The loading of the "Context" is handled by the "topbanner"
+ //loadCrazyflieContext();
+ //}
+
+ // ADD KEYBOARD SHORTCUTS
+ // > For "all motors off", press the space bar
+ ui->motors_off_button->setShortcut(tr("Space"));
+}
+
+ConnectStartStopBar::~ConnectStartStopBar()
+{
+ delete ui;
+}
+
+
+
+// > For making the "enable flight" and "disable flight" buttons unavailable
+void ConnectStartStopBar::disableFlyingStateButtons()
+{
+ ui->motors_off_button->setEnabled(true);
+ ui->enable_flying_button->setEnabled(false);
+ ui->disable_flying_button->setEnabled(false);
+}
+
+// > For making the "enable flight" and "disable flight" buttons available
+void ConnectStartStopBar::enableFlyingStateButtons()
+{
+ ui->motors_off_button->setEnabled(true);
+ ui->enable_flying_button->setEnabled(true);
+ ui->disable_flying_button->setEnabled(true);
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC RRRR A ZZZZZ Y Y RRRR A DDDD III OOO
+// C R R A A Z Y Y R R A A D D I O O
+// C RRRR A A Z Y RRRR A A D D I O O
+// C R R AAAAA Z Y R R AAAAA D D I O O
+// CCCC R R A A ZZZZZ Y R R A A DDDD III OOO
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+// > For the Battery Voltage
+void ConnectStartStopBar::crazyRadioStatusCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Crazy Radio Status Callback called for agentID = " << m_agentID);
+ setCrazyRadioStatus( msg.data );
+}
+#endif
+
+
+// PRIVATE METHODS FOR SETTING PROPERTIES
+
+void ConnectStartStopBar::setCrazyRadioStatus(int new_radio_status)
+{
+ // add more things whenever the status is changed
+ switch(new_radio_status)
+ {
+ case CRAZY_RADIO_STATE_CONNECTED:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
+ //m_rf_status_label_mutex.lock();
+ //ui->rf_status_label->clear();
+ QPixmap rf_connected_pixmap(":/images/rf_connected.png");
+ ui->rf_status_label->setPixmap(rf_connected_pixmap);
+ ui->rf_status_label->setScaledContents(true);
+ //ui->rf_status_label->update();
+ //m_rf_status_label_mutex.unlock();
+
+ // ENABLE THE REMAINDER OF THE GUI
+ if (m_type == TYPE_AGENT)
+ {
+ enableFlyingStateButtons();
+ }
+ break;
+ }
+
+ case CRAZY_RADIO_STATE_CONNECTING:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
+ //m_rf_status_label_mutex.lock();
+ //ui->rf_status_label->clear();
+ QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
+ ui->rf_status_label->setPixmap(rf_connecting_pixmap);
+ ui->rf_status_label->setScaledContents(true);
+ //ui->rf_status_label->update();
+ //m_rf_status_label_mutex.unlock();
+ break;
+ }
+
+ case CRAZY_RADIO_STATE_DISCONNECTED:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
+ //m_rf_status_label_mutex.lock();
+ //ui->rf_status_label->clear();
+ QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
+ ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
+ ui->rf_status_label->setScaledContents(true);
+ //ui->rf_status_label->update();
+ //m_rf_status_label_mutex.unlock();
+
+ // DISABLE THE REMAINDER OF THE GUI
+ if (m_type == TYPE_AGENT)
+ {
+ disableFlyingStateButtons();
+ }
+ break;
+ }
+
+ default:
+ {
+ break;
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+// > For the Battery Voltage
+void ConnectStartStopBar::batteryVoltageCallback(const std_msgs::Float32& msg)
+{
+ //setBatteryVoltageTextAndImage( msg.data );
+ setBatteryVoltageText( msg.data );
+}
+
+
+void ConnectStartStopBar::batteryStateChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Battery State Changed Callback called for agentID = " << m_agentID);
+ setBatteryState( msg.data );
+}
+
+
+void ConnectStartStopBar::batteryLevelCallback(const std_msgs::Int32& msg)
+{
+ setBatteryImageBasedOnLevel( msg.data );
+}
+#endif
+
+
+
+// PRIVATE METHODS FOR SETTING PROPERTIES
+
+// > For updating the battery state
+void ConnectStartStopBar::setBatteryState(int new_battery_state)
+{
+ // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
+ m_battery_state = new_battery_state;
+}
+
+
+
+// > For updating the battery voltage shown in the UI elements of "battery_voltage_lineEdit"
+void ConnectStartStopBar::setBatteryVoltageText(float battery_voltage)
+{
+ // Lock the mutex
+ //m_battery_voltage_lineEdit_mutex.lock();
+ // Construct the text string
+ QString qstr = "";
+ if (battery_voltage >= 0.0f)
+ {
+ qstr.append(QString::number(battery_voltage, 'f', 2));
+ }
+ else
+ {
+ qstr.append("-.--");
+ }
+ qstr.append(" V");
+ // Set the text to the battery voltage line edit
+ ui->battery_voltage_lineEdit->setText(qstr);
+ // Unlock the mutex
+ //m_battery_voltage_lineEdit_mutex.unlock();
+}
+
+
+
+// > For updating the battery image shown in the UI element of "battery_status_label"
+void ConnectStartStopBar::setBatteryImageBasedOnLevel(int battery_level)
+{
+ // COMPUTE THE BATTERY VOLTAGE AS A PERCENTAGE
+ //float battery_voltage_percentage = fromVoltageToPercent(battery_voltage);
+
+ // CONVERT THE VOLTAGE LEVEL TO AN INDEX OF WHICH BATTERY LEVEL IMAGE TO DISPLAY
+ // > Initialise a local variable that will be set in the switch case below
+ int new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
+ // > Initialise a local variable for the string of which image to use
+ QString qstr_new_image = "";
+ qstr_new_image.append(":/images/");
+
+ // Fill in these two local variables accordingly
+ switch(battery_level)
+ {
+ case BATTERY_LEVEL_000:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
+ qstr_new_image.append("battery_empty.png");
+ break;
+ }
+ case BATTERY_LEVEL_010:
+ case BATTERY_LEVEL_020:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_20;
+ qstr_new_image.append("battery_20.png");
+ break;
+ }
+ case BATTERY_LEVEL_030:
+ case BATTERY_LEVEL_040:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_40;
+ qstr_new_image.append("battery_40.png");
+ break;
+ }
+ case BATTERY_LEVEL_050:
+ case BATTERY_LEVEL_060:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_60;
+ qstr_new_image.append("battery_60.png");
+ break;
+ }
+ case BATTERY_LEVEL_070:
+ case BATTERY_LEVEL_080:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_80;
+ qstr_new_image.append("battery_80.png");
+ break;
+ }
+ case BATTERY_LEVEL_090:
+ case BATTERY_LEVEL_100:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_FULL;
+ qstr_new_image.append("battery_full.png");
+ break;
+ }
+ case BATTERY_LEVEL_UNAVAILABLE:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNVAILABLE;
+ qstr_new_image.append("battery_unavailable.png");
+ break;
+ }
+ default:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
+ qstr_new_image.append("battery_unknown.png");
+ break;
+ }
+ }
+ // UPDATE THE IMAGE DISPLAYED BASED ON THE "new index"
+ // > Only if it is different from the current index
+ m_battery_status_label_mutex.lock();
+ if (m_battery_status_label_image_current_index != new_image_index)
+ {
+ // SET THE IMAGE FOR THE BATTERY STATUS LABEL
+ ui->battery_status_label->clear();
+ QPixmap battery_image_pixmap(qstr_new_image);
+ ui->battery_status_label->setPixmap(battery_image_pixmap);
+ ui->battery_status_label->setScaledContents(true);
+ m_battery_status_label_image_current_index = new_image_index;
+ ui->battery_status_label->update();
+ }
+ m_battery_status_label_mutex.unlock();
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF L Y Y III GGGG SSSS TTTTT A TTTTT EEEE
+// F L Y Y I G S T A A T E
+// FFF L Y I G SSS T A A T EEE
+// F L Y I G G S T AAAAA T E
+// F LLLLL Y III GGGG SSSS T A A T EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+// RESPONDING TO CHANGES IN THE FLYING STATE
+#ifdef CATKIN_MAKE
+void ConnectStartStopBar::flyingStateChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Flying State Changed Callback called for agentID = " << m_agentID);
+ setFlyingState(msg.data);
+}
+#endif
+
+void ConnectStartStopBar::setFlyingState(int new_flying_state)
+{
+ // UPDATE THE LABEL TO DISPLAY THE FLYING STATE
+ switch(new_flying_state)
+ {
+ case STATE_MOTORS_OFF:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_off_pixmap(":/images/flying_state_off.png");
+ ui->flying_state_label->setPixmap(flying_state_off_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_TAKE_OFF:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_enabling_pixmap(":/images/flying_state_enabling.png");
+ ui->flying_state_label->setPixmap(flying_state_enabling_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_FLYING:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_flying_pixmap(":/images/flying_state_flying.png");
+ ui->flying_state_label->setPixmap(flying_state_flying_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_LAND:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_disabling_pixmap(":/images/flying_state_disabling.png");
+ ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_UNAVAILABLE:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_disabling_pixmap(":/images/flying_state_unavailable.png");
+ ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ default:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_unknown_pixmap(":/images/flying_state_unknown.png");
+ ui->flying_state_label->setPixmap(flying_state_unknown_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+
+void ConnectStartStopBar::on_rf_connect_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_RECONNECT;
+ this->crazyRadioCommandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Connect button clicked");
+#endif
+}
+
+void ConnectStartStopBar::on_rf_disconnect_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_DISCONNECT;
+ this->crazyRadioCommandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Disconnect button clicked");
+#endif
+}
+
+void ConnectStartStopBar::on_enable_flying_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_CRAZYFLY_TAKE_OFF;
+ this->flyingStateCommandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable flying button clicked");
+#endif
+}
+
+void ConnectStartStopBar::on_disable_flying_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_CRAZYFLY_LAND;
+ this->flyingStateCommandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Disable flying button clicked");
+#endif
+}
+
+void ConnectStartStopBar::on_motors_off_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ this->flyingStateCommandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Motors-off button clicked");
+#endif
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+ // > Request the current flying state
+ ros::ServiceClient getCurrentFlyingStateService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/getCurrentFlyingState", false);
+ dfall_pkg::IntIntService getFlyingStateCall;
+ getFlyingStateCall.request.data = 0;
+ getCurrentFlyingStateService.waitForExistence(ros::Duration(2.0));
+ if(getCurrentFlyingStateService.call(getFlyingStateCall))
+ {
+ setFlyingState(getFlyingStateCall.response.data);
+ }
+ else
+ {
+ setFlyingState(STATE_UNAVAILABLE);
+ }
+
+ // > Request the current status of the crazy radio
+ ros::ServiceClient getCurrentCrazyRadioStateService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("CrazyRadio/getCurrentCrazyRadioStatus", false);
+ dfall_pkg::IntIntService getCrazyRadioCall;
+ getCrazyRadioCall.request.data = 0;
+ getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(2.0));
+ if(getCurrentCrazyRadioStateService.call(getCrazyRadioCall))
+ {
+ setCrazyRadioStatus(getCrazyRadioCall.response.data);
+ }
+ else
+ {
+ setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+ }
+
+ // > For updating the "rf_status_label" picture
+ crazyRadioStatusSubscriber = agent_base_nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &ConnectStartStopBar::crazyRadioStatusCallback, this);
+
+ // > For updating the current battery voltage
+ batteryVoltageSubscriber = agent_base_nodeHandle.subscribe("BatteryMonitor/FilteredVoltage", 1, &ConnectStartStopBar::batteryVoltageCallback, this);
+
+ // > For updating the current battery state
+ //batteryStateSubscriber = agent_base_nodeHandle.subscribe("BatteryMonitor/ChangedStateTo", 1, &ConnectStartStopBar::batteryStateChangedCallback, this);
+
+ // > For updating the current battery level
+ batteryLevelSubscriber = agent_base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &ConnectStartStopBar::batteryLevelCallback, this);
+
+ // > For updating the "flying_state_label" picture
+ flyingStateSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ crazyRadioStatusSubscriber.shutdown();
+ batteryVoltageSubscriber.shutdown();
+ //batteryStateSubscriber.shutdown();
+ batteryLevelSubscriber.shutdown();
+ flyingStateSubscriber.shutdown();
+
+ // Set information back to the default
+ setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+ setBatteryVoltageText(-1.0f);
+ setBatteryImageBasedOnLevel(BATTERY_LEVEL_UNAVAILABLE);
+ setFlyingState(STATE_UNAVAILABLE);
+
+ }
+#endif
+
+
+#ifdef CATKIN_MAKE
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CONNECT START STOP GUI BAR] is coordinating agentIDs:";
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ QTextStream(stdout) << " " << agentIDs[irow];
+ }
+ QTextStream(stdout) << " " << endl;
+#endif
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
+// MM MM S G G H H E A A D D E R R
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the head for a message
+void ConnectStartStopBar::fillIntMessageHeader( dfall_pkg::IntWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool ConnectStartStopBar::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[CONNECT START STOP GUI BAR] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[CONNECT START STOP GUI BAR] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[CONNECT START STOP GUI BAR] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[CONNECT START STOP GUI BAR] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[CONNECT START STOP GUI BAR] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[CONNECT START STOP GUI BAR] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[CONNECT START STOP GUI BAR] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3ee513c9d267622fcdbf28e69d0aa71c3f2aa3e6
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
@@ -0,0 +1,647 @@
+// Copyright (C) 2019, 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 tab wdiget that contains and manages the individual tabs used
+// to interface with each separate controller.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "controllertabs.h"
+#include "ui_controllertabs.h"
+
+ControllerTabs::ControllerTabs(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::ControllerTabs)
+{
+ ui->setupUi(this);
+
+
+ // Specify the color for normal and highlighted tabs
+ m_tab_text_colour_normal = Qt::black;
+ m_tab_text_colour_highlight = QColor(0,200,0);
+
+
+ // Initialise the object name as blank
+ m_object_name_for_emitting_pose_data = "";
+
+
+ // CONNECT THE "MEASURED POST" SIGNAL TO EACH OF
+ // THE TABS
+ // i.e., connect the "measured pose value changed"
+ // signal to the "set measured pose" slots
+ QObject::connect(
+ this , &ControllerTabs::measuredPoseValueChanged ,
+ ui->default_controller_tab_widget , &DefaultControllerTab::setMeasuredPose
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::measuredPoseValueChanged ,
+ ui->student_controller_tab_widget , &StudentControllerTab::setMeasuredPose
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::measuredPoseValueChanged ,
+ ui->picker_controller_tab_widget , &PickerControllerTab::setMeasuredPose
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::measuredPoseValueChanged ,
+ ui->tuning_controller_tab_widget , &TuningControllerTab::setMeasuredPose
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::measuredPoseValueChanged ,
+ ui->template_controller_tab_widget , &TemplateControllerTab::setMeasuredPose
+ );
+
+
+
+ // CONNECT THE "MEASUREMENTS UNAVAILABLE" SIGNAL TO
+ // EACH OF THE TABS
+ QObject::connect(
+ this , &ControllerTabs::poseDataUnavailableSignal ,
+ ui->default_controller_tab_widget , &DefaultControllerTab::poseDataUnavailableSlot
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::poseDataUnavailableSignal ,
+ ui->student_controller_tab_widget , &StudentControllerTab::poseDataUnavailableSlot
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::poseDataUnavailableSignal ,
+ ui->picker_controller_tab_widget , &PickerControllerTab::poseDataUnavailableSlot
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::poseDataUnavailableSignal ,
+ ui->tuning_controller_tab_widget , &TuningControllerTab::poseDataUnavailableSlot
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::poseDataUnavailableSignal ,
+ ui->template_controller_tab_widget , &TemplateControllerTab::poseDataUnavailableSlot
+ );
+
+
+ // CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
+ // WITH THE LIST OF AGENT IDs TO COORDINATE
+ // This is passed from this "controller tabs widget" to
+ // each of the controller tabs. The signal is simply
+ // "passed through"
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->default_controller_tab_widget , &DefaultControllerTab::setAgentIDsToCoordinate
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->student_controller_tab_widget , &StudentControllerTab::setAgentIDsToCoordinate
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->picker_controller_tab_widget , &PickerControllerTab::setAgentIDsToCoordinate
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->tuning_controller_tab_widget , &TuningControllerTab::setAgentIDsToCoordinate
+ );
+
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->template_controller_tab_widget , &TemplateControllerTab::setAgentIDsToCoordinate
+ );
+
+
+
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CONTROLLER TABS GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[CONTROLLER TABS GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE A NODE HANDLE TO THE D-FaLL ROOT
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+ // CREATE THE SUBSCRIBER TO THE MOTION CAPTURE DATA
+ m_poseDataSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &ControllerTabs::poseDataReceivedCallback, this);
+
+ // CREATE THE SUBSCRIBER TO THE CONTROLLER THAT IS CURRENTLY OPERATING
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
+ }
+
+
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle dfall_root_nodeHandle("/dfall");
+
+ // > Publisher for the emergency stop button
+ //emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("emergencyStop", 1);
+
+ // > For changes in the database that defines {agentID,cfID,flying zone} links
+ //databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);;
+ centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<dfall_pkg::CMQuery>("CentralManagerService/Query", false);
+
+
+#endif
+
+}
+
+ControllerTabs::~ControllerTabs()
+{
+ delete ui;
+}
+
+
+
+void ControllerTabs::showHideController_toggle(QString qstr_label, QWidget * tab_widget_to_toggle)
+{
+ // Get the current index of the tab
+ // > Note the this returns -1 if the tab is not found
+ int current_index_of_tab = ui->controller_tabs_widget->indexOf(tab_widget_to_toggle);
+
+ // Switch depending on whether the tab was found
+ if (current_index_of_tab < 0)
+ {
+ // Insert the tab
+ ui->controller_tabs_widget->addTab(tab_widget_to_toggle,qstr_label);
+ }
+ else
+ {
+ // Remove the tab
+ ui->controller_tabs_widget->removeTab(current_index_of_tab);
+ }
+}
+
+
+
+void ControllerTabs::showHideController_default_changed()
+{
+ showHideController_toggle("Default",ui->default_tab);
+}
+
+void ControllerTabs::showHideController_student_changed()
+{
+ showHideController_toggle("Student",ui->student_tab);
+}
+
+void ControllerTabs::showHideController_picker_changed()
+{
+ showHideController_toggle("Picker",ui->picker_tab);
+}
+
+void ControllerTabs::showHideController_tuning_changed()
+{
+ showHideController_toggle("Tuning",ui->tuning_tab);
+}
+
+void ControllerTabs::showHideController_template_changed()
+{
+ showHideController_toggle("Template",ui->template_tab);
+}
+
+
+
+
+
+
+void ControllerTabs::setObjectNameForDisplayingPoseData( QString object_name )
+{
+ m_should_search_pose_data_for_object_name_mutex.lock();
+ if (object_name.isEmpty())
+ {
+ // Set the class variable accordingly
+ m_object_name_for_emitting_pose_data = "";
+ // Update the flag accordingly
+ m_should_search_pose_data_for_object_name = false;
+ // Emit a signal to let the tabs know
+ emit poseDataUnavailableSignal();
+ // Inform the user
+ #ifdef CATKIN_MAKE
+ ROS_INFO("[CONTROLLER TABS GUI] No longer emitting pose data for any object.");
+ #endif
+ }
+ else
+ {
+ // Set the class variable accordingly
+ m_object_name_for_emitting_pose_data = object_name.toStdString();
+ // Update the flag accordingly
+ m_should_search_pose_data_for_object_name = true;
+ // Inform the user
+ #ifdef CATKIN_MAKE
+ ROS_INFO_STREAM("[CONTROLLER TABS GUI] now emitting data for object named: " << m_object_name_for_emitting_pose_data << ", with ID = " << m_ID );
+ #endif
+
+
+ #ifdef CATKIN_MAKE
+ // Get also the context
+ dfall_pkg::CMQuery contextCall;
+ contextCall.request.studentID = m_ID;
+
+ centralManagerDatabaseService.waitForExistence(ros::Duration(-1));
+
+ if(centralManagerDatabaseService.call(contextCall))
+ {
+ m_context = contextCall.response.crazyflieContext;
+ m_area = m_context.localArea;
+ ROS_INFO_STREAM("[CONTROLLER TABS GUI] AreaBounds:\n" << m_area);
+
+ //qstr_crazyflie_name.append(QString::fromStdString(m_context.crazyflieName));
+
+ //m_object_name_for_emitting_pose_data = QString::fromStdString(my_context.crazyflieName);
+
+ }
+ else
+ {
+ ROS_ERROR_STREAM("[CONTROLLER TABS GUI] Failed to load context for agentID = " << m_ID);
+ }
+ #endif
+
+ }
+ m_should_search_pose_data_for_object_name_mutex.unlock();
+}
+
+
+#ifdef CATKIN_MAKE
+// > For the controller currently operating, received on
+// "controllerUsedSubscriber"
+void ControllerTabs::poseDataReceivedCallback(const dfall_pkg::ViconData& viconData)
+{
+ m_should_search_pose_data_for_object_name_mutex.lock();
+ if (m_should_search_pose_data_for_object_name)
+ {
+ for(std::vector<dfall_pkg::CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
+ {
+ dfall_pkg::CrazyflieData pose_in_global_frame = *it;
+
+ if(pose_in_global_frame.crazyflieName == m_object_name_for_emitting_pose_data)
+ {
+
+ // Convert it into the local frame
+ float originX = (m_area.xmin + m_area.xmax) / 2.0;
+ float originY = (m_area.ymin + m_area.ymax) / 2.0;
+
+ pose_in_global_frame.x -= originX;
+ pose_in_global_frame.y -= originY;
+
+ // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
+ //float originZ = 0.0;
+ // float originZ = (area.zmin + area.zmax) / 2.0;
+ //pose_in_global_frame.z -= originZ;
+
+ emit measuredPoseValueChanged(
+ pose_in_global_frame.x,
+ pose_in_global_frame.y,
+ pose_in_global_frame.z,
+ pose_in_global_frame.roll,
+ pose_in_global_frame.pitch,
+ pose_in_global_frame.yaw,
+ pose_in_global_frame.occluded
+ );
+ }
+ }
+ }
+ m_should_search_pose_data_for_object_name_mutex.unlock();
+}
+#endif
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC OOO N N TTTTT RRRR OOO L L EEEEE RRRR
+// C O O NN N T R R O O L L E R R
+// C O O N N N T RRRR O O L L EEE RRRR
+// C O O N NN T R R O O L L E R R
+// CCCC OOO N N T R R OOO LLLLL LLLLL EEEEE R R
+//
+// EEEEE N N A BBBB L EEEEE DDDD
+// E NN N A A B B L E D D
+// EEE N N N A A BBBB L EEE D D
+// E N NN AAAAA B B L E D D
+// EEEEE N N A A BBBB LLLLL EEEEE DDDD
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// > For the controller currently operating, received on "controllerUsedSubscriber"
+void ControllerTabs::controllerUsedChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Controller Used Changed Callback called for agentID = " << m_agentID);
+ setControllerEnabled(msg.data);
+}
+#endif
+
+
+void ControllerTabs::setControllerEnabled(int new_controller)
+{
+ // First set everything back to normal colouring
+ setAllTabLabelsToNormalColouring();
+
+ // Now switch to highlight the tab corresponding to
+ // the enable controller
+ switch(new_controller)
+ {
+ case DEFAULT_CONTROLLER:
+ {
+ setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->default_tab );
+ break;
+ }
+ case DEMO_CONTROLLER:
+ {
+ //ui->controller_enabled_label->setText("Demo");
+ break;
+ }
+ case STUDENT_CONTROLLER:
+ {
+ setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->student_tab );
+ break;
+ }
+ case MPC_CONTROLLER:
+ {
+ //ui->controller_enabled_label->setText("MPC");
+ break;
+ }
+ case REMOTE_CONTROLLER:
+ {
+ //ui->controller_enabled_label->setText("Remote");
+ break;
+ }
+ case TUNING_CONTROLLER:
+ {
+ setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->tuning_tab );
+ break;
+ }
+ case PICKER_CONTROLLER:
+ {
+ setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->picker_tab );
+ break;
+ }
+ case TEMPLATE_CONTROLLER:
+ {
+ setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->template_tab );
+ break;
+ }
+ default:
+ {
+ //ui->controller_enabled_label->setText("Unknown");
+ break;
+ }
+ }
+}
+
+
+void ControllerTabs::setAllTabLabelsToNormalColouring()
+{
+ setTextColourOfTabLabel( m_tab_text_colour_normal , ui->default_tab );
+ setTextColourOfTabLabel( m_tab_text_colour_normal , ui->student_tab );
+ setTextColourOfTabLabel( m_tab_text_colour_normal , ui->picker_tab );
+ setTextColourOfTabLabel( m_tab_text_colour_normal , ui->tuning_tab );
+ setTextColourOfTabLabel( m_tab_text_colour_normal , ui->template_tab );
+}
+
+void ControllerTabs::setTextColourOfTabLabel(QColor color , QWidget * tab_widget)
+{
+ // Get the current index of the tab
+ int current_index_of_tab = ui->controller_tabs_widget->indexOf(tab_widget);
+ // Onlz apply the colour is the tab is found
+ if (current_index_of_tab >= 0)
+ {
+ ui->controller_tabs_widget->tabBar()->setTabTextColor(current_index_of_tab, color);
+ }
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+ // Pass on the signal to the tabs
+ emit agentIDsToCoordinateChanged( agentIDs , shouldCoordinateAll );
+
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+
+ // > Request the current instant controller
+ ros::ServiceClient getInstantControllerService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/getInstantController", false);
+ dfall_pkg::IntIntService getInstantControllerCall;
+ getInstantControllerCall.request.data = 0;
+ getInstantControllerService.waitForExistence(ros::Duration(2.0));
+ if(getInstantControllerService.call(getInstantControllerCall))
+ {
+ setControllerEnabled(getInstantControllerCall.response.data);
+ }
+ else
+ {
+ //setControllerEnabled(CONTROLLER_UNKNOWN);
+ }
+
+ // SUBSCRIBERS
+ // > For receiving message that the instant controller was changed
+ controllerUsedSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ controllerUsedSubscriber.shutdown();
+
+ // Set all tabs to be normal colours
+ setAllTabLabelsToNormalColouring();
+
+ }
+#endif
+
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool ControllerTabs::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[DEFAULT CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinator.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f7f25b1625299bc79ef5e9f69ef2a1222669ab14
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinator.cpp
@@ -0,0 +1,294 @@
+// Copyright (C) 2019, 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 coordinator part of the Flying Agent GUI that allows multiple
+// flying agents to be controller from one place, and keeps track
+// of which subset of flying agents to control.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "coordinator.h"
+#include "ui_coordinator.h"
+
+Coordinator::Coordinator(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::Coordinator)
+{
+ ui->setupUi(this);
+
+ ui->verticalLayout_for_coordinatedAgentsScrollArea->setAlignment(Qt::AlignTop);
+}
+
+Coordinator::~Coordinator()
+{
+ delete ui;
+}
+
+void Coordinator::on_refresh_button_clicked()
+{
+ // DELETE ALL EXISTING ROWS
+ remove_all_entries_from_vector_of_coordinatorRows();
+
+ // Get the state of the "coordinate all" is check box
+ bool shouldCoordinateAll = ui->coordinate_all_checkBox->isChecked();
+
+
+#ifdef CATKIN_MAKE
+ // USE A REGULAR EXPRESSION TO IDENTIFY NODES THAT EXIST
+ // > The regular expression we use is: \/agent(\d\d\d)\/FlyingAgentClient
+ // with the different that the escaped character is \\ instead of \ only
+
+ // GET A "V_string" OF ALL THE NODES CURRENTLY IN EXISTENCE
+ ros::V_string v_str;
+ ros::master::getNodes(v_str);
+
+ // ITERATE THROUGH THIS "V_string" OF ALL NODE IN EXISTENCE
+ for(int i = 0; i < v_str.size(); i++)
+ {
+ // TEST THE NAME OF THIS NODE AGAINST THE REGULAR EXPRESSION
+ std::string s = v_str[i];
+ std::smatch m;
+ std::regex e ("\\/agent(\\d\\d\\d)\\/FlyingAgentClient");
+
+ if(std::regex_search(s, m, e))
+ {
+ // GET THE MATCHED PART OF THE NODE NAME INTO A LOCAL VARIABLE
+ // > Note: we use m[1] because we are interested ONLY in the first match
+ // > Thus "found_string" should the the agentID, as a string with zero padding
+ std::string found_string = m[1].str();
+
+
+ // LET THE USER KNOW THAT WE FOUND A MATCH
+ ROS_INFO_STREAM("[Coordinator Row GUI] Found an agent with ID = " << found_string.c_str() );
+
+ // CONVERT THE STRING TO AN INTEGER
+ int this_agentID = stoi(found_string);
+
+ // ADD A ROW TO THE COORDINATOR FOR THE AGENT WITH THIS ID FOUND
+ CoordinatorRow *temp_coordinatorRow = new CoordinatorRow(this,this_agentID);
+
+ // Check the box if "coordinate all" is checked
+ temp_coordinatorRow->setShouldCoordinate(shouldCoordinateAll);
+
+ // Connect the "should coordinate value changed" signal to
+ // the respective slot
+ QObject::connect(
+ temp_coordinatorRow , &CoordinatorRow::shouldCoordinateThisAgentValueChanged ,
+ this , &Coordinator::setShouldCoordinateThisAgent
+ );
+
+ // Add to the vector of coordinator rows
+ vector_of_coordinatorRows.append(temp_coordinatorRow);
+ vector_of_shouldCoordinate_perRow.append(shouldCoordinateAll);
+ vector_of_agentID_perRow.append(this_agentID);
+
+ ui->coordinated_agents_scrollAreaWidgetContents->layout()->addWidget(temp_coordinatorRow);
+ }
+ }
+#else
+
+
+ for ( int i_agent = 1 ; i_agent < 9 ; i_agent++ )
+ {
+ //ui->scrollAreaWidgetContents->setLayout(new QVBoxLayout);
+
+ CoordinatorRow *temp_coordinatorRow = new CoordinatorRow(this,i_agent);
+
+ // Check the box if "coordinate all" is checked
+ temp_coordinatorRow->setShouldCoordinate(shouldCoordinateAll);
+
+ // Connect the "should coordinate value changed" signal to
+ // the respective slot
+ QObject::connect(
+ temp_coordinatorRow , &CoordinatorRow::shouldCoordinateThisAgentValueChanged ,
+ this , &Coordinator::setShouldCoordinateThisAgent
+ );
+
+ // Add to the vector of coordinator rows
+ vector_of_coordinatorRows.append(temp_coordinatorRow);
+ vector_of_shouldCoordinate_perRow.append(shouldCoordinateAll);
+ vector_of_agentID_perRow.append(i_agent);
+
+ ui->coordinated_agents_scrollAreaWidgetContents->layout()->addWidget(temp_coordinatorRow);
+ }
+#endif
+
+ // Send out a signal with the current IDs to coordinate
+ emit_signal_with_agentIDs_toCoordinate();
+
+ // Call the function that applies this level
+ // of detail to all the entries
+ apply_level_of_detail_to_all_entries(level_of_detail_to_display);
+}
+
+
+void Coordinator::on_toggle_details_button_clicked()
+{
+ // Toggle the level of detail to display
+ switch (level_of_detail_to_display)
+ {
+ case 0:
+ {
+ level_of_detail_to_display = 1;
+ break;
+ }
+ case 1:
+ {
+ level_of_detail_to_display = 0;
+ break;
+ }
+ default:
+ {
+ level_of_detail_to_display = 0;
+ break;
+ }
+ }
+ // Call the function that applies this level
+ // of detail to all the entries
+ apply_level_of_detail_to_all_entries(level_of_detail_to_display);
+}
+
+void Coordinator::on_delete_button_clicked()
+{
+ // Call the function that performs the task requested
+ remove_all_entries_from_vector_of_coordinatorRows();
+
+ // Send out a signal with the current IDs to coordinate
+ emit_signal_with_agentIDs_toCoordinate();
+}
+
+void Coordinator::remove_all_entries_from_vector_of_coordinatorRows()
+{
+ // Iterate through and delete all entries
+ foreach ( CoordinatorRow* temp_coordinatorRow, vector_of_coordinatorRows) {
+ delete( temp_coordinatorRow );
+ }
+ // Clear the vector
+ vector_of_coordinatorRows.clear();
+ vector_of_shouldCoordinate_perRow.clear();
+ vector_of_agentID_perRow.clear();
+}
+
+void Coordinator::apply_level_of_detail_to_all_entries(int level)
+{
+ // Apply this to all the rows
+ for ( int irow = 0 ; irow < vector_of_coordinatorRows.length() ; irow++ )
+ {
+ vector_of_coordinatorRows[irow]->setLevelOfDetailToDisplay( level );
+ }
+}
+
+void Coordinator::on_coordinate_all_checkBox_clicked()
+{
+ // Get the state of the "coordinate all" is check box
+ bool shouldCoordinateAll = ui->coordinate_all_checkBox->isChecked();
+
+ // Apply this to all the rows
+ for ( int irow = 0 ; irow < vector_of_coordinatorRows.length() ; irow++ )
+ {
+ vector_of_coordinatorRows[irow]->setShouldCoordinate( shouldCoordinateAll );
+ vector_of_shouldCoordinate_perRow[irow] = shouldCoordinateAll;
+ }
+
+ // Send out a signal with the current IDs to coordinate
+ emit_signal_with_agentIDs_toCoordinate();
+}
+
+
+void Coordinator::setShouldCoordinateThisAgent(int agentID , bool shouldCoordinate)
+{
+ // Find the row with the matching ID, and update the "shouldCoordinate" vector
+ for ( int irow = 0 ; irow < vector_of_agentID_perRow.length() ; irow++ )
+ {
+ if (vector_of_agentID_perRow[irow] == agentID)
+ {
+ vector_of_shouldCoordinate_perRow[irow] = shouldCoordinate;
+ break;
+ }
+ }
+
+ // Update the "Coordinate All Check Box" as appropriate
+ bool shouldCoordinateAll = true;
+ if (!shouldCoordinate)
+ {
+ shouldCoordinateAll = false;
+ }
+ else
+ {
+ for ( int irow = 0 ; irow < vector_of_shouldCoordinate_perRow.length() ; irow++ )
+ {
+ if (!(vector_of_shouldCoordinate_perRow[irow]))
+ {
+ shouldCoordinateAll = false;
+ break;
+ }
+ }
+ }
+ ui->coordinate_all_checkBox->setChecked(shouldCoordinateAll);
+
+ // Send out a signal with the current IDs to coordinate
+ emit_signal_with_agentIDs_toCoordinate();
+}
+
+void Coordinator::emit_signal_with_agentIDs_toCoordinate()
+{
+ // Initilise a boolean for whether to coordinate all
+ bool shouldCoordinateAll = true;
+ // Send out a signal with the current IDs to coordinate
+ QVector<int> agentIDsToCoordinate;
+ for ( int irow = 0 ; irow < vector_of_shouldCoordinate_perRow.length() ; irow++ )
+ {
+ if (vector_of_shouldCoordinate_perRow[irow])
+ {
+ agentIDsToCoordinate.append(vector_of_agentID_perRow[irow]);
+ }
+ else
+ {
+ shouldCoordinateAll = false;
+ }
+ }
+ emit agentIDsToCoordinateChanged( agentIDsToCoordinate , shouldCoordinateAll );
+
+
+#ifdef CATKIN_MAKE
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[COORDINATOR] is coordinating agentIDs:";
+ for ( int irow = 0 ; irow < vector_of_shouldCoordinate_perRow.length() ; irow++ )
+ {
+ if (vector_of_shouldCoordinate_perRow[irow])
+ {
+ QTextStream(stdout) << " " << vector_of_agentID_perRow[irow];
+ }
+ }
+ QTextStream(stdout) << " " << endl;
+#endif
+}
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6d60f69027d02249278daa7a449ac0c111beb90
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
@@ -0,0 +1,825 @@
+// Copyright (C) 2019, 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:
+// An individual row in the coordinator part of the Flying Agent
+// GUI. This is essentially a condensed version of the
+// "Connect Start Stop Bar"
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "coordinatorrow.h"
+#include "ui_coordinatorrow.h"
+
+
+
+
+
+CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
+ QWidget(parent),
+ ui(new Ui::CoordinatorRow),
+ m_agentID(agentID)
+{
+ ui->setupUi(this);
+
+ // CONVERT THE AGENT ID TO A ZERO PADDED STRING
+ // > This is the c++ method:
+ //std::ostringstream str_stream;
+ //str_stream << std::setw(3) << std::setfill('0') << m_agentID;
+ //std::string agentID_as_string(str_stream.str());
+ // > This is the Qt method
+ //m_agentID_as_string = QString("%1").arg(m_agentID, 3, 10, QChar('0'));
+ // For which the syntax is:
+ // - Arg1: the number
+ // - Arg2: how many 0 you want?
+ // - Arg3: The base (10 - decimal, 16 hexadecimal)
+ // > Alternate Qt method:
+ m_agentID_as_string = QString::number(m_agentID).rightJustified(3, '0');
+
+ // CONVERT THE AGENT ID TO A STRING FOR THE BASE NAMESPACE
+ QString qstr_base_namespace = "/dfall/agent";
+ qstr_base_namespace.append(m_agentID_as_string);
+ std::string base_namespace = qstr_base_namespace.toStdString();
+
+ // SET THE INITIAL VALUE OF THE PRIVATE VARIABLES FOR THIS CLASS
+ // > For keeping track of the current battery state
+ m_battery_state = BATTERY_STATE_NORMAL;
+ // > For keeping track of which image is currently displayed
+ m_battery_status_label_image_current_index = -999;
+
+
+ // SET THE STARTING RADIO STATUS TO BE: DISCONNECTED
+ setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+
+ // SET THE STARTING BATTERY VOLTAGE TO BE: UNKNOWN
+ setBatteryVoltageText(-1.0f);
+
+ // SET THE STARTING BATTERY LEVEL TO BE: UNAVAILABLE
+ setBatteryImageBasedOnLevel(BATTERY_LEVEL_UNAVAILABLE);
+
+ // SET THE STARTING FLYING STATE STATUS TO BE: MOTORS OFF
+ setFlyingState(STATE_UNAVAILABLE);
+
+ // SET THE DEFAULT NAME FOR THE SELECTED CONTROLLER
+ setControllerEnabled(DEFAULT_CONTROLLER);
+
+
+#ifdef CATKIN_MAKE
+
+ // LET THE USER KNOW WHAT THE BASE NAMESPACE IS
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] using base namespace: " << base_namespace.c_str() << ", for agentID = " << m_agentID);
+
+ // DEBUGGING FOR NAMESPACES
+ //std::string temp_ros_namespace = ros::this_node::getNamespace();
+ //ROS_INFO_STREAM("[COORDINATOR ROW GUI] compared to: ros::this_node::getNamespace() = " << temp_ros_namespace.c_str());
+
+ // CREATE A NODE HANDLE TO THE BASE NAMESPACE
+ ros::NodeHandle base_nodeHandle(base_namespace);
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle dfall_root_nodeHandle("/dfall");
+
+ // SUBSCRIBERS AND PUBLISHERS:
+
+ // > For Crazyradio commands based on button clicks
+ crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/crazyRadioCommand", 1);
+ // > For updating the "rf_status_label" picture
+ crazyRadioStatusSubscriber = base_nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &CoordinatorRow::crazyRadioStatusCallback, this);
+
+ // > For updating the current battery voltage
+ batteryVoltageSubscriber = base_nodeHandle.subscribe("BatteryMonitor/FilteredVoltage", 1, &CoordinatorRow::batteryVoltageCallback, this);
+ // > For updating the current battery state
+ //batteryStateSubscriber = base_nodeHandle.subscribe("BatteryMonitor/ChangedStateTo", 1, &CoordinatorRow::batteryStateChangedCallback, this);
+ // > For updating the current battery level
+ batteryLevelSubscriber = base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &CoordinatorRow::batteryLevelCallback, this);
+
+ // > For Flying state commands based on button clicks
+ flyingStateCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1);
+ // > For updating the "flying_state_label" picture
+ flyingStateSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/flyingState", 1, &CoordinatorRow::flyingStateChangedCallback, this);
+
+ // > For changes in the database that defines {agentID,cfID,flying zone} links
+ databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &CoordinatorRow::databaseChangedCallback, this);;
+ centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<dfall_pkg::CMQuery>("CentralManagerService/Query", false);
+
+ // > For updating the controller that is currently operating
+ controllerUsedSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/controllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this);
+
+ // > For requesting the current flying state,
+ // this is used only for initialising the icon
+ getCurrentFlyingStateService = base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/getCurrentFlyingState", false);
+
+ // > For requesting the current state of the Crazy Radio,
+ // this is used only for initialising the icon
+ getCurrentCrazyRadioStateService = base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("CrazyRadio/getCurrentCrazyRadioStatus", false);
+
+#endif
+
+ // FURTHER INITILIASATIONS NEED TO OCCUR AFTER THE ROS RELATED
+ // INITIALISATIONS ARE COMPLETE
+ loadCrazyflieContext();
+
+ // > Request the current flying state
+ getCurrentFlyingState();
+
+ // > Request the current state of the Crazy Radio
+ getCurrentCrazyRadioState();
+
+}
+
+CoordinatorRow::~CoordinatorRow()
+{
+ delete ui;
+}
+
+
+
+// PUBLIC METHODS FOR SETTING PROPERTIES
+
+// > Set the state of the checkbox
+void CoordinatorRow::setShouldCoordinate(bool shouldCoordinate)
+{
+ ui->shouldCoordinate_checkBox->setChecked( shouldCoordinate );
+}
+
+void CoordinatorRow::setLevelOfDetailToDisplay(int level)
+{
+ switch (level)
+ {
+ case 0:
+ {
+ ui->motors_off_button->setVisible(true);
+ ui->enable_flying_button->setVisible(true);
+ ui->disable_flying_button->setVisible(true);
+
+ ui->battery_voltage_lineEdit->setVisible(true);
+
+ ui->rf_connect_button->setVisible(true);
+ ui->rf_disconnect_button->setVisible(true);
+
+ QString qstr_for_checkbox_label = "ID";
+ qstr_for_checkbox_label.append(QString::number(m_agentID));
+ qstr_for_checkbox_label.append(", ");
+ qstr_for_checkbox_label.append(m_crazyflie_name_as_string);
+ ui->shouldCoordinate_checkBox->setText(qstr_for_checkbox_label);
+ break;
+ }
+ case 1:
+ {
+ ui->motors_off_button->setVisible(false);
+ ui->enable_flying_button->setVisible(false);
+ ui->disable_flying_button->setVisible(false);
+
+ ui->battery_voltage_lineEdit->setVisible(false);
+
+ ui->rf_connect_button->setVisible(false);
+ ui->rf_disconnect_button->setVisible(false);
+
+ QString qstr_for_checkbox_label = "ID";
+ qstr_for_checkbox_label.append(QString::number(m_agentID));
+ ui->shouldCoordinate_checkBox->setText(qstr_for_checkbox_label);
+
+
+ break;
+ }
+ default:
+ {
+ ui->motors_off_button->setVisible(true);
+ ui->enable_flying_button->setVisible(true);
+ ui->disable_flying_button->setVisible(true);
+
+ ui->battery_voltage_lineEdit->setVisible(true);
+
+ ui->rf_connect_button->setVisible(true);
+ ui->rf_disconnect_button->setVisible(true);
+
+ QString qstr_for_checkbox_label = "ID";
+ qstr_for_checkbox_label.append(QString::number(m_agentID));
+ qstr_for_checkbox_label.append(", ");
+ qstr_for_checkbox_label.append(m_crazyflie_name_as_string);
+ ui->shouldCoordinate_checkBox->setText(qstr_for_checkbox_label);
+ break;
+ }
+ }
+}
+
+
+// > For making the "enable flight" and "disable flight" buttons unavailable
+void CoordinatorRow::disableFlyingStateButtons()
+{
+ ui->motors_off_button->setEnabled(true);
+ ui->enable_flying_button->setEnabled(false);
+ ui->disable_flying_button->setEnabled(false);
+}
+
+// > For making the "enable flight" and "disable flight" buttons available
+void CoordinatorRow::enableFlyingStateButtons()
+{
+ ui->motors_off_button->setEnabled(true);
+ ui->enable_flying_button->setEnabled(true);
+ ui->disable_flying_button->setEnabled(true);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC RRRR A ZZZZZ Y Y RRRR A DDDD III OOO
+// C R R A A Z Y Y R R A A D D I O O
+// C RRRR A A Z Y RRRR A A D D I O O
+// C R R AAAAA Z Y R R AAAAA D D I O O
+// CCCC R R A A ZZZZZ Y R R A A DDDD III OOO
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+// > For the Battery Voltage
+void CoordinatorRow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Crazy Radio Status Callback called for agentID = " << m_agentID);
+ setCrazyRadioStatus( msg.data );
+}
+#endif
+
+
+// PRIVATE METHODS FOR SETTING PROPERTIES
+
+void CoordinatorRow::setCrazyRadioStatus(int new_radio_status)
+{
+ // add more things whenever the status is changed
+ switch(new_radio_status)
+ {
+ case CRAZY_RADIO_STATE_CONNECTED:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
+ //m_rf_status_label_mutex.lock();
+ //ui->rf_status_label->clear();
+ QPixmap rf_connected_pixmap(":/images/rf_connected.png");
+ ui->rf_status_label->setPixmap(rf_connected_pixmap);
+ ui->rf_status_label->setScaledContents(true);
+ //ui->rf_status_label->update();
+ //m_rf_status_label_mutex.unlock();
+
+ // ENABLE THE REMAINDER OF THE GUI
+ enableFlyingStateButtons();
+ break;
+ }
+
+ case CRAZY_RADIO_STATE_CONNECTING:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
+ //m_rf_status_label_mutex.lock();
+ //ui->rf_status_label->clear();
+ QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
+ ui->rf_status_label->setPixmap(rf_connecting_pixmap);
+ ui->rf_status_label->setScaledContents(true);
+ //ui->rf_status_label->update();
+ //m_rf_status_label_mutex.unlock();
+ break;
+ }
+
+ case CRAZY_RADIO_STATE_DISCONNECTED:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
+ //m_rf_status_label_mutex.lock();
+ //ui->rf_status_label->clear();
+ QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
+ ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
+ ui->rf_status_label->setScaledContents(true);
+ //ui->rf_status_label->update();
+ //m_rf_status_label_mutex.unlock();
+
+ // DISABLE THE REMAINDER OF THE GUI
+ disableFlyingStateButtons();
+ break;
+ }
+
+ default:
+ {
+ break;
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+
+// > For the Battery Voltage
+void CoordinatorRow::batteryVoltageCallback(const std_msgs::Float32& msg)
+{
+ //setBatteryVoltageTextAndImage( msg.data );
+ setBatteryVoltageText( msg.data );
+}
+
+
+void CoordinatorRow::batteryStateChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Battery State Changed Callback called for agentID = " << m_agentID);
+ setBatteryState( msg.data );
+}
+
+
+void CoordinatorRow::batteryLevelCallback(const std_msgs::Int32& msg)
+{
+ setBatteryImageBasedOnLevel( msg.data );
+}
+#endif
+
+
+
+// PRIVATE METHODS FOR SETTING PROPERTIES
+
+// > For updating the battery state
+void CoordinatorRow::setBatteryState(int new_battery_state)
+{
+ // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
+ m_battery_state = new_battery_state;
+}
+
+
+
+// > For updating the battery voltage shown in the UI elements of "battery_voltage_lineEdit"
+void CoordinatorRow::setBatteryVoltageText(float battery_voltage)
+{
+ // Lock the mutex
+ //m_battery_voltage_lineEdit_mutex.lock();
+ // Construct the text string
+ QString qstr = "";
+ if (battery_voltage >= 0.0f)
+ {
+ qstr.append(QString::number(battery_voltage, 'f', 2));
+ }
+ else
+ {
+ qstr.append("-.--");
+ }
+ qstr.append(" V");
+ // Set the text to the battery voltage line edit
+ ui->battery_voltage_lineEdit->setText(qstr);
+ // Unlock the mutex
+ //m_battery_voltage_lineEdit_mutex.unlock();
+}
+
+
+
+// > For updating the battery image shown in the UI element of "battery_status_label"
+void CoordinatorRow::setBatteryImageBasedOnLevel(int battery_level)
+{
+ // COMPUTE THE BATTERY VOLTAGE AS A PERCENTAGE
+ //float battery_voltage_percentage = fromVoltageToPercent(battery_voltage);
+
+ // CONVERT THE VOLTAGE LEVEL TO AN INDEX OF WHICH BATTERY LEVEL IMAGE TO DISPLAY
+ // > Initialise a local variable that will be set in the switch case below
+ int new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
+ // > Initialise a local variable for the string of which image to use
+ QString qstr_new_image = "";
+ qstr_new_image.append(":/images/");
+
+ // Fill in these two local variables accordingly
+ switch(battery_level)
+ {
+ case BATTERY_LEVEL_000:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
+ qstr_new_image.append("battery_empty.png");
+ break;
+ }
+ case BATTERY_LEVEL_010:
+ case BATTERY_LEVEL_020:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_20;
+ qstr_new_image.append("battery_20.png");
+ break;
+ }
+ case BATTERY_LEVEL_030:
+ case BATTERY_LEVEL_040:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_40;
+ qstr_new_image.append("battery_40.png");
+ break;
+ }
+ case BATTERY_LEVEL_050:
+ case BATTERY_LEVEL_060:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_60;
+ qstr_new_image.append("battery_60.png");
+ break;
+ }
+ case BATTERY_LEVEL_070:
+ case BATTERY_LEVEL_080:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_80;
+ qstr_new_image.append("battery_80.png");
+ break;
+ }
+ case BATTERY_LEVEL_090:
+ case BATTERY_LEVEL_100:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_FULL;
+ qstr_new_image.append("battery_full.png");
+ break;
+ }
+ case BATTERY_LEVEL_UNAVAILABLE:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNVAILABLE;
+ qstr_new_image.append("battery_unavailable.png");
+ break;
+ }
+ default:
+ {
+ new_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
+ qstr_new_image.append("battery_unknown.png");
+ break;
+ }
+ }
+ // UPDATE THE IMAGE DISPLAYED BASED ON THE "new index"
+ // > Only if it is different from the current index
+ m_battery_status_label_mutex.lock();
+ if (m_battery_status_label_image_current_index != new_image_index)
+ {
+ // SET THE IMAGE FOR THE BATTERY STATUS LABEL
+ ui->battery_status_label->clear();
+ QPixmap battery_image_pixmap(qstr_new_image);
+ ui->battery_status_label->setPixmap(battery_image_pixmap);
+ ui->battery_status_label->setScaledContents(true);
+ m_battery_status_label_image_current_index = new_image_index;
+ ui->battery_status_label->update();
+ }
+ m_battery_status_label_mutex.unlock();
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF L Y Y III GGGG SSSS TTTTT A TTTTT EEEE
+// F L Y Y I G S T A A T E
+// FFF L Y I G SSS T A A T EEE
+// F L Y I G G S T AAAAA T E
+// F LLLLL Y III GGGG SSSS T A A T EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+// RESPONDING TO CHANGES IN THE FLYING STATE
+#ifdef CATKIN_MAKE
+void CoordinatorRow::flyingStateChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Flying State Changed Callback called for agentID = " << m_agentID);
+ setFlyingState(msg.data);
+}
+#endif
+
+void CoordinatorRow::setFlyingState(int new_flying_state)
+{
+ // UPDATE THE LABEL TO DISPLAY THE FLYING STATE
+ switch(new_flying_state)
+ {
+ case STATE_MOTORS_OFF:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_off_pixmap(":/images/flying_state_off.png");
+ ui->flying_state_label->setPixmap(flying_state_off_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_TAKE_OFF:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_enabling_pixmap(":/images/flying_state_enabling.png");
+ ui->flying_state_label->setPixmap(flying_state_enabling_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_FLYING:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_flying_pixmap(":/images/flying_state_flying.png");
+ ui->flying_state_label->setPixmap(flying_state_flying_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_LAND:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_disabling_pixmap(":/images/flying_state_disabling.png");
+ ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ case STATE_UNAVAILABLE:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_disabling_pixmap(":/images/flying_state_unavailable.png");
+ ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+
+ default:
+ {
+ // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+ QPixmap flying_state_unknown_pixmap(":/images/flying_state_unknown.png");
+ ui->flying_state_label->setPixmap(flying_state_unknown_pixmap);
+ ui->flying_state_label->setScaledContents(true);
+ break;
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC OOO N N TTTTT EEEEE X X TTTTT
+// C O O NN N T E X X T
+// C O O N N N T EEE X T
+// C O O N NN T E X X T
+// CCCC OOO N N T EEEEE X X T
+// ----------------------------------------------------------------------------------
+
+
+
+// RESPONDING TO CHANGES IN THE DATABASE
+#ifdef CATKIN_MAKE
+// > For the notification that the database was changes, received on the "DatabaseChangedSubscriber"
+void CoordinatorRow::databaseChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Database Changed Callback called for agentID = " << m_agentID);
+ loadCrazyflieContext();
+}
+#endif
+
+// > For loading the "context" for this agent, i.e., the {agentID,cfID,flying zone} tuple
+void CoordinatorRow::loadCrazyflieContext()
+{
+ QString qstr_crazyflie_name = "";
+#ifdef CATKIN_MAKE
+ dfall_pkg::CMQuery contextCall;
+ contextCall.request.studentID = m_agentID;
+ //ROS_INFO_STREAM("StudentID:" << m_agentID);
+
+ centralManagerDatabaseService.waitForExistence(ros::Duration(-1));
+
+ if(centralManagerDatabaseService.call(contextCall))
+ {
+ my_context = contextCall.response.crazyflieContext;
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] CrazyflieContext:\n" << my_context);
+
+ qstr_crazyflie_name.append(QString::fromStdString(my_context.crazyflieName));
+ }
+ else
+ {
+ ROS_ERROR_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Failed to load context for agentID = " << m_agentID);
+ }
+ // This updating of the radio only needs to be done by the actual agent's node
+ //ros::NodeHandle nh("CrazyRadio");
+ //nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
+#else
+ // Set the Crazyflie Name String to be a question mark
+ qstr_crazyflie_name.append("?");
+#endif
+
+ // Construct and set the string for the checkbox label
+ QString qstr_for_checkbox_label = "ID";
+ qstr_for_checkbox_label.append(QString::number(m_agentID));
+ qstr_for_checkbox_label.append(", ");
+ qstr_for_checkbox_label.append(qstr_crazyflie_name);
+ ui->shouldCoordinate_checkBox->setText(qstr_for_checkbox_label);
+
+ // Set the name of the Crazyflie to the class variable
+ m_crazyflie_name_as_string = qstr_crazyflie_name;
+}
+
+
+
+void CoordinatorRow::getCurrentFlyingState()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntIntService getFlyingStateCall;
+ getFlyingStateCall.request.data = 0;
+ getCurrentFlyingStateService.waitForExistence(ros::Duration(2.0));
+ if(getCurrentFlyingStateService.call(getFlyingStateCall))
+ {
+ setFlyingState(getFlyingStateCall.response.data);
+ }
+ else
+ {
+ setFlyingState(STATE_UNAVAILABLE);
+ }
+#endif
+}
+
+
+
+
+void CoordinatorRow::getCurrentCrazyRadioState()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntIntService getCrazyRadioCall;
+ getCrazyRadioCall.request.data = 0;
+ getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(2.0));
+ if(getCurrentCrazyRadioStateService.call(getCrazyRadioCall))
+ {
+ setCrazyRadioStatus(getCrazyRadioCall.response.data);
+ }
+ else
+ {
+ setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+ }
+#endif
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC OOO N N TTTTT RRRR OOO L L EEEEE RRRR
+// C O O NN N T R R O O L L E R R
+// C O O N N N T RRRR O O L L EEE RRRR
+// C O O N NN T R R O O L L E R R
+// CCCC OOO N N T R R OOO LLLLL LLLLL EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// > For the controller currently operating, received on "controllerUsedSubscriber"
+void CoordinatorRow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[COORDINATOR ROW GUI] Controller Used Changed Callback called for agentID = " << m_agentID);
+ setControllerEnabled(msg.data);
+}
+#endif
+
+
+void CoordinatorRow::setControllerEnabled(int new_controller)
+{
+ switch(new_controller)
+ {
+ case DEFAULT_CONTROLLER:
+ {
+ ui->controller_enabled_label->setText("Default");
+ break;
+ }
+ case DEMO_CONTROLLER:
+ {
+ ui->controller_enabled_label->setText("Demo");
+ break;
+ }
+ case STUDENT_CONTROLLER:
+ {
+ ui->controller_enabled_label->setText("Student");
+ break;
+ }
+ case MPC_CONTROLLER:
+ {
+ ui->controller_enabled_label->setText("MPC");
+ break;
+ }
+ case REMOTE_CONTROLLER:
+ {
+ ui->controller_enabled_label->setText("Remote");
+ break;
+ }
+ case TUNING_CONTROLLER:
+ {
+ ui->controller_enabled_label->setText("Tuning");
+ break;
+ }
+ default:
+ {
+ ui->controller_enabled_label->setText("Unknown");
+ break;
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+
+void CoordinatorRow::on_rf_connect_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ msg.shouldCheckForAgentID = false;
+ msg.data = CMD_RECONNECT;
+ this->crazyRadioCommandPublisher.publish(msg);
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Connect button clicked");
+#endif
+}
+
+void CoordinatorRow::on_rf_disconnect_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ msg.shouldCheckForAgentID = false;
+ msg.data = CMD_DISCONNECT;
+ this->crazyRadioCommandPublisher.publish(msg);
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Disconnect button clicked");
+#endif
+}
+
+void CoordinatorRow::on_enable_flying_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ msg.shouldCheckForAgentID = false;
+ msg.data = CMD_CRAZYFLY_TAKE_OFF;
+ this->flyingStateCommandPublisher.publish(msg);
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Enable flying button clicked");
+#endif
+}
+
+void CoordinatorRow::on_disable_flying_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ msg.shouldCheckForAgentID = false;
+ msg.data = CMD_CRAZYFLY_LAND;
+ this->flyingStateCommandPublisher.publish(msg);
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Disable flying button clicked");
+#endif
+}
+
+void CoordinatorRow::on_motors_off_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ msg.shouldCheckForAgentID = false;
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ this->flyingStateCommandPublisher.publish(msg);
+ ROS_INFO_STREAM("[COORDINATOR ROW agentID:" << m_agentID << " GUI] Motors-off button clicked");
+#endif
+}
+
+void CoordinatorRow::on_shouldCoordinate_checkBox_clicked()
+{
+ // Get the state of the "should coordinate" check box
+ bool shouldCoordinate = ui->shouldCoordinate_checkBox->isChecked();
+
+ // Send out a signal with this information
+ emit shouldCoordinateThisAgentValueChanged( m_agentID , shouldCoordinate );
+}
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4170841a634141710fce641984b6e8f3f6dbf1ac
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
@@ -0,0 +1,865 @@
+// Copyright (C) 2019, 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 GUI for the Default Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "defaultcontrollertab.h"
+#include "ui_defaultcontrollertab.h"
+
+DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::DefaultControllerTab)
+{
+ ui->setupUi(this);
+
+ // Hide the two red frames that are used to indcated
+ // when pose data is occluded
+ ui->red_frame_position_left->setVisible(false);
+ ui->red_frame_position_right->setVisible(false);
+
+ // Make the current state label blank
+ ui->label_current_state->setText("");
+
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
+ requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("DefaultControllerService/RequestSetpointChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("DefaultControllerService/SetpointChanged", 1, &DefaultControllerTab::setpointChangedCallback, this);
+ }
+
+ // GET THE CURRENT SETPOINT
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("DefaultControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+ }
+
+#endif
+
+}
+
+DefaultControllerTab::~DefaultControllerTab()
+{
+ delete ui;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// PPPP OOO SSSS EEEEE DDDD A TTTTT A
+// P P O O S E D D A A T A A
+// PPPP O O SSS EEE D D A A T A A
+// P O O S E D D AAAAA T AAAAA
+// P OOO SSSS EEEEE DDDD A A T A A
+// ----------------------------------------------------------------------------------
+
+
+void DefaultControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+{
+ if (!occluded)
+ {
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+ // UPDATE THE MEASUREMENT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_x->setText(qstr + QString::number( x, 'f', 3));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_y->setText(qstr + QString::number( y, 'f', 3));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', 3));
+
+ if (roll < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_roll->setText(qstr + QString::number( roll * RAD2DEG, 'f', 1));
+ if (pitch < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_pitch->setText(qstr + QString::number( pitch * RAD2DEG, 'f', 1));
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+
+ // GET THE CURRENT SETPOINT ERROR
+ float error_x = x - (ui->lineEdit_setpoint_current_x->text() ).toFloat();
+ float error_y = y - (ui->lineEdit_setpoint_current_y->text() ).toFloat();
+ float error_z = z - (ui->lineEdit_setpoint_current_z->text() ).toFloat();
+
+ float error_yaw_deg = yaw * RAD2DEG - (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+ // UPDATE THE ERROR COLUMN
+ if (error_x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_x->setText(qstr + QString::number( error_x, 'f', 3));
+ if (error_y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_y->setText(qstr + QString::number( error_y, 'f', 3));
+ if (error_z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', 3));
+
+ if (error_yaw_deg < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw_deg , 'f', 1));
+
+ // Ensure the red frames are not visible
+ if ( ui->red_frame_position_left->isVisible() )
+ ui->red_frame_position_left->setVisible(false);
+ if ( ui->red_frame_position_right->isVisible() )
+ ui->red_frame_position_right->setVisible(false);
+ }
+ else
+ {
+ // Make visible the red frames to indicate occluded
+ if ( !(ui->red_frame_position_left->isVisible()) )
+ ui->red_frame_position_left->setVisible(true);
+ if ( !(ui->red_frame_position_right->isVisible()) )
+ ui->red_frame_position_right->setVisible(true);
+ }
+}
+
+
+void DefaultControllerTab::poseDataUnavailableSlot()
+{
+ ui->lineEdit_measured_x->setText("xx.xx");
+ ui->lineEdit_measured_y->setText("xx.xx");
+ ui->lineEdit_measured_z->setText("xx.xx");
+
+ ui->lineEdit_measured_roll->setText("xx.xx");
+ ui->lineEdit_measured_pitch->setText("xx.xx");
+ ui->lineEdit_measured_yaw->setText("xx.xx");
+
+ ui->lineEdit_error_x->setText("xx.xx");
+ ui->lineEdit_error_y->setText("xx.xx");
+ ui->lineEdit_error_z->setText("xx.xx");
+ ui->lineEdit_error_yaw->setText("xx.xx");
+
+
+}
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+//
+// CCCC H H A N N GGGG EEEEE DDDD
+// C H H A A NN N G E D D
+// C HHHHH A A N N N G EEE D D
+// C H H AAAAA N NN G G E D D
+// CCCC H H A A N N GGGG EEEEE DDDD
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void DefaultControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
+{
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+
+ // EXTRACT THE SETPOINT
+ float x = newSetpoint.x;
+ float y = newSetpoint.y;
+ float z = newSetpoint.z;
+ float yaw = newSetpoint.yaw;
+
+ // UPDATE THE SETPOINT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_x->setText(qstr + QString::number( x, 'f', 3));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_y->setText(qstr + QString::number( y, 'f', 3));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_z->setText(qstr + QString::number( z, 'f', 3));
+
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 3));
+
+ // UPDATE THE CURRENT STATE LABEL
+ int current_state = newSetpoint.buttonID;
+ m_label_current_state_mutex.lock();
+ switch (current_state)
+ {
+ case DEFAULT_CONTROLLER_STATE_NORMAL:
+ {
+ ui->label_current_state->setText("normal");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_STANDBY:
+ {
+ ui->label_current_state->setText("standby");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_SPIN_MOTORS:
+ {
+ ui->label_current_state->setText("Take-off, spinning motors");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_MOVE_UP:
+ {
+ ui->label_current_state->setText("Take-off, moving up");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT:
+ {
+ ui->label_current_state->setText("Take-off, goto setpoint");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_INTEGRATOR_ON:
+ {
+ ui->label_current_state->setText("Take-off, integrator on");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN:
+ {
+ ui->label_current_state->setText("Landing, move down");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS:
+ {
+ ui->label_current_state->setText("Landing, spinning motors");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_UNKNOWN:
+ {
+ ui->label_current_state->setText("Unknown");
+ break;
+ }
+ default:
+ {
+ ui->label_current_state->setText("Not recognised");
+ break;
+ }
+ }
+ m_label_current_state_mutex.unlock();
+}
+#endif
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE QQQ U U EEEEE SSSS TTTTT N N EEEEE W W
+// R R E Q Q U U E S T NN N E W W
+// RRRR EEE Q Q U U EEE SSS T N N N EEE W W
+// R R E Q Q U U E S T N NN E W W W
+// R R EEEEE QQ Q UUU EEEEE SSSS T N N EEEEE W W
+//
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+// ----------------------------------------------------------------------------------
+
+
+void DefaultControllerTab::publishSetpoint(float x, float y, float z, float yaw)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw * DEG2RAD;
+
+ // Publish the setpoint
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Published request for setpoint change to: [" << x << ", "<< y << ", "<< z << ", "<< yaw << "]");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[DEFAULT CONTROLLER GUI] would publish request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw << "]";
+#endif
+}
+
+
+
+void DefaultControllerTab::on_lineEdit_setpoint_new_x_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+
+#ifdef CATKIN_MAKE
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[DEFAULT CONTROLLER TAB] return pressed for x setpoint";
+#endif
+}
+
+void DefaultControllerTab::on_lineEdit_setpoint_new_y_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void DefaultControllerTab::on_lineEdit_setpoint_new_z_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void DefaultControllerTab::on_lineEdit_setpoint_new_yaw_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void DefaultControllerTab::on_set_setpoint_button_clicked()
+{
+
+ // Initialise local variable for each of (x,y,z,yaw)
+ float x = 0.0f, y = 0.0f, z = 0.0f, yaw = 0.0f;
+
+ // Take the new value if available, otherwise use the old value
+ // > For x
+ if(!ui->lineEdit_setpoint_new_x->text().isEmpty())
+ x = (ui->lineEdit_setpoint_new_x->text()).toFloat();
+ else
+ x = (ui->lineEdit_setpoint_current_x->text()).toFloat();
+ // > For x
+ if(!ui->lineEdit_setpoint_new_y->text().isEmpty())
+ y = (ui->lineEdit_setpoint_new_y->text()).toFloat();
+ else
+ y = (ui->lineEdit_setpoint_current_y->text()).toFloat();
+ // > For x
+ if(!ui->lineEdit_setpoint_new_z->text().isEmpty())
+ z = (ui->lineEdit_setpoint_new_z->text()).toFloat();
+ else
+ z = (ui->lineEdit_setpoint_current_z->text()).toFloat();
+ // > For x
+ if(!ui->lineEdit_setpoint_new_yaw->text().isEmpty())
+ yaw = (ui->lineEdit_setpoint_new_yaw->text()).toFloat();
+ else
+ yaw = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+#ifdef CATKIN_MAKE
+ // Call the function to publish the setpoint
+ publishSetpoint(x,y,z,yaw);
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[DEFAULT CONTROLLER TAB] set setpoint button clicked";
+#endif
+}
+
+void DefaultControllerTab::on_default_setpoint_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Publish this as a blank setpoint with the
+ // "buttonID" field set appropriately
+
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.buttonID = REQUEST_DEFAULT_SETPOINT_BUTTON_ID;
+
+ // Publish the default setpoint button press
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Published request for setpoint change to the default");
+#endif
+}
+
+void DefaultControllerTab::on_x_increment_plus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_x->text().isEmpty())
+ {
+ // Call the function to publish the setpoint
+ publishSetpoint(
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat() + (ui->lineEdit_setpoint_increment_x->text()).toFloat(),
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_yaw->text()).toFloat()
+ );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+
+ }
+}
+
+void DefaultControllerTab::on_x_increment_minus_button_clicked()
+{
+
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_x->text().isEmpty())
+ {
+ // Call the function to publish the setpoint
+ publishSetpoint(
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat() - (ui->lineEdit_setpoint_increment_x->text()).toFloat(),
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_yaw->text()).toFloat()
+ );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void DefaultControllerTab::on_y_increment_plus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_y->text().isEmpty())
+ {
+ // Call the function to publish the setpoint
+ publishSetpoint(
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat() + (ui->lineEdit_setpoint_increment_y->text()).toFloat(),
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_yaw->text()).toFloat()
+ );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void DefaultControllerTab::on_y_increment_minus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_y->text().isEmpty())
+ {
+ // Call the function to publish the setpoint
+ publishSetpoint(
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat() - (ui->lineEdit_setpoint_increment_y->text()).toFloat(),
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_yaw->text()).toFloat()
+ );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void DefaultControllerTab::on_z_increment_plus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_z->text().isEmpty())
+ {
+ // Call the function to publish the setpoint
+ publishSetpoint(
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat() + (ui->lineEdit_setpoint_increment_z->text()).toFloat(),
+ (ui->lineEdit_setpoint_current_yaw->text()).toFloat()
+ );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void DefaultControllerTab::on_z_increment_minus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_z->text().isEmpty())
+ {
+ // Call the function to publish the setpoint
+ publishSetpoint(
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat() - (ui->lineEdit_setpoint_increment_z->text()).toFloat(),
+ (ui->lineEdit_setpoint_current_yaw->text()).toFloat()
+ );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void DefaultControllerTab::on_yaw_increment_plus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
+ {
+ // Call the function to publish the setpoint
+ publishSetpoint(
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_yaw->text()).toFloat() + (ui->lineEdit_setpoint_increment_yaw->text()).toFloat()
+ );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+ }
+}
+void DefaultControllerTab::on_yaw_increment_minus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
+ {
+ // Call the function to publish the setpoint
+ publishSetpoint(
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat(),
+ (ui->lineEdit_setpoint_current_yaw->text()).toFloat() - (ui->lineEdit_setpoint_increment_yaw->text()).toFloat()
+ );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void DefaultControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+ // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("DefaultControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+
+ // SUBSCRIBERS
+ // > For receiving message that the setpoint was changed
+ setpointChangedSubscriber = agent_base_nodeHandle.subscribe("DefaultControllerService/SetpointChanged", 1, &DefaultControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // Set information back to the default
+ ui->lineEdit_setpoint_current_x->setText("xx.xx");
+ ui->lineEdit_setpoint_current_y->setText("xx.xx");
+ ui->lineEdit_setpoint_current_z->setText("xx.xx");
+ ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
+
+ }
+#endif
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
+// MM MM S G G H H E A A D D E R R
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the head for a message
+void DefaultControllerTab::fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool DefaultControllerTab::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[DEFAULT CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[DEFAULT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d5389a8bf817fb1bbaf74c9c4404151a614650b2
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
@@ -0,0 +1,501 @@
+// Copyright (C) 2019, 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 GUI bar for selecting which controller is active, and
+// for request that paramters are loaded from the respective
+// YAML files.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "enablecontrollerloadyamlbar.h"
+#include "ui_enablecontrollerloadyamlbar.h"
+
+EnableControllerLoadYamlBar::EnableControllerLoadYamlBar(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::EnableControllerLoadYamlBar)
+{
+ ui->setupUi(this);
+
+
+#ifdef CATKIN_MAKE
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE COMMAND PUBLISHER
+ commandPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1);
+
+ // CREATE THE REQUEST LOAD YAML FILE PUBLISHER
+ // Get the node handle to this parameter service
+ m_requestLoadYamlFilenamePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::StringWithHeader>("ParameterService/requestLoadYamlFilename", 1);
+#endif
+
+}
+
+EnableControllerLoadYamlBar::~EnableControllerLoadYamlBar()
+{
+ delete ui;
+}
+
+
+
+
+void EnableControllerLoadYamlBar::showHideController_default_changed()
+{
+ ui->enable_default_button ->setHidden( !(ui->enable_default_button->isHidden()) );
+ ui->load_yaml_default_button->setHidden( !(ui->load_yaml_default_button->isHidden()) );
+}
+
+void EnableControllerLoadYamlBar::showHideController_student_changed()
+{
+ ui->enable_student_button ->setHidden( !(ui->enable_student_button->isHidden()) );
+ ui->load_yaml_student_button->setHidden( !(ui->load_yaml_student_button->isHidden()) );
+}
+
+void EnableControllerLoadYamlBar::showHideController_picker_changed()
+{
+ ui->enable_picker_button ->setHidden( !(ui->enable_picker_button->isHidden()) );
+ ui->load_yaml_picker_button->setHidden( !(ui->load_yaml_picker_button->isHidden()) );
+}
+
+void EnableControllerLoadYamlBar::showHideController_tuning_changed()
+{
+ ui->enable_tuning_button ->setHidden( !(ui->enable_tuning_button->isHidden()) );
+ ui->load_yaml_tuning_button->setHidden( !(ui->load_yaml_tuning_button->isHidden()) );
+}
+
+void EnableControllerLoadYamlBar::showHideController_template_changed()
+{
+ ui->enable_template_button ->setHidden( !(ui->enable_template_button->isHidden()) );
+ ui->load_yaml_template_button->setHidden( !(ui->load_yaml_template_button->isHidden()) );
+}
+
+
+
+
+
+// ENABLE CONTROLLER BUTTONS ON-CLICK CALLBACK
+
+void EnableControllerLoadYamlBar::on_enable_default_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_USE_DEFAULT_CONTROLLER;
+ this->commandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Default Controller");
+#endif
+}
+
+void EnableControllerLoadYamlBar::on_enable_student_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_USE_STUDENT_CONTROLLER;
+ this->commandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Student Controller");
+#endif
+}
+
+void EnableControllerLoadYamlBar::on_enable_picker_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_USE_PICKER_CONTROLLER;
+ this->commandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Picker Controller");
+#endif
+}
+
+void EnableControllerLoadYamlBar::on_enable_tuning_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_USE_TUNING_CONTROLLER;
+ this->commandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Tuning Controller");
+#endif
+}
+
+void EnableControllerLoadYamlBar::on_enable_template_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_USE_TEMPLATE_CONTROLLER;
+ this->commandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Template Controller");
+#endif
+}
+
+
+
+
+
+
+
+// LOAD YAML BUTTONS ON-CLICK CALLBACK
+
+void EnableControllerLoadYamlBar::on_load_yaml_default_button_clicked()
+{
+ #ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ dfall_pkg::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "DefaultController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Default Controller YAML was clicked.");
+#endif
+}
+
+void EnableControllerLoadYamlBar::on_load_yaml_student_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ dfall_pkg::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "StudentController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Student Controller YAML was clicked.");
+#endif
+}
+
+void EnableControllerLoadYamlBar::on_load_yaml_picker_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ dfall_pkg::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "PickerController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Picker Controller YAML was clicked.");
+#endif
+}
+
+void EnableControllerLoadYamlBar::on_load_yaml_tuning_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ dfall_pkg::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "TuningController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Tuning Controller YAML was clicked.");
+#endif
+}
+
+void EnableControllerLoadYamlBar::on_load_yaml_template_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ dfall_pkg::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "TemplateController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Template Controller YAML was clicked.");
+#endif
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void EnableControllerLoadYamlBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
+// MM MM S G G H H E A A D D E R R
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the head for a message
+void EnableControllerLoadYamlBar::fillIntMessageHeader( dfall_pkg::IntWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the head for a message
+void EnableControllerLoadYamlBar::fillStringMessageHeader( dfall_pkg::StringWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool EnableControllerLoadYamlBar::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/main.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/main.cpp
similarity index 94%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/main.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/main.cpp
index ceb21be31d17e24f190406845a27f74cc1c197d8..4d01be0bdd2687811a3f6bf504493d159312f763 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/main.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/main.cpp
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
@@ -25,7 +25,7 @@
//
//
// DESCRIPTION:
-// Main file of the Qt project for the Coordinator GUI.
+// Main file of the Qt project for the Flying Agent GUI.
//
// ----------------------------------------------------------------------------------
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a523fe34ee7aa2199456e18ef1cea04d7fa64e9
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
@@ -0,0 +1,354 @@
+// Copyright (C) 2019, 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 Flying Agent GUI main window.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+
+MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
+ QMainWindow(parent),
+ ui(new Ui::MainWindow)
+{
+#ifdef CATKIN_MAKE
+ m_rosNodeThread = new rosNodeThread(argc, argv, "FlyingAgentGUI");
+#endif
+
+#ifdef CATKIN_MAKE
+ m_rosNodeThread->init();
+#endif
+ ui->setupUi(this);
+
+ // ADD KEYBOARD SHORTCUTS
+ // > For "kill GUI node", press "CTRL+C" while the GUI window is the focus
+ m_close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
+
+#ifdef CATKIN_MAKE
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[FLYING AGENT GUI MAIN WINDOW] ros::this_node::getNamespace() = " << this_namespace);
+
+
+ // Get the type and ID of this parameter service
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the TYPE and ID are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[FLYING AGENT GUI MAIN WINDOW] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+ // 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 "ParameterService" node
+ std::string this_node_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[FLYING AGENT GUI MAIN WINDOW] ros::this_node::getNamespace() = " << this_node_namespace);
+
+ // Construct the string to the namespace of this Paramater Service
+ m_parameter_service_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[FLYING AGENT GUI MAIN WINDOW] parameter service is: " << m_parameter_service_namespace);
+
+ // Get the node handle to this parameter service
+ ros::NodeHandle nodeHandle_to_parameter_service(m_parameter_service_namespace);
+ m_requestLoadYamlFilenamePublisher = nodeHandle_to_parameter_service.advertise<StringWithHeader>("requestLoadYamlFilename", 1);
+
+ // Remove the show/hide coordinator menu item if launch as an agent
+ if (m_type==TYPE_AGENT)
+ {
+ // Hide the coordinator part of the GUI
+ ui->customWidget_coordinator->hide();
+ // And make the menu item unavailable
+ ui->actionShowHide_Coordinator->setEnabled(false);
+
+ }
+#endif
+
+ // CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
+ // WITH THE LIST OF AGENT IDs TO COORDINATE
+ // This is passed from the "coordinator" to the elements
+ // in the main part of the GUI, namely to the:
+ // > "top banner",
+ // > "connect,start,stop bar",
+ // > "enable controller, load yaml bar", and
+ // > "controller tabs widget".
+ QObject::connect(
+ ui->customWidget_coordinator , &Coordinator::agentIDsToCoordinateChanged ,
+ ui->customWidget_topBanner , &TopBanner::setAgentIDsToCoordinate
+ );
+
+ QObject::connect(
+ ui->customWidget_coordinator , &Coordinator::agentIDsToCoordinateChanged ,
+ ui->customWidget_connectStartStopBar , &ConnectStartStopBar::setAgentIDsToCoordinate
+ );
+
+ QObject::connect(
+ ui->customWidget_coordinator , &Coordinator::agentIDsToCoordinateChanged ,
+ ui->customWidget_enableControllerLoadYamlBar , &EnableControllerLoadYamlBar::setAgentIDsToCoordinate
+ );
+
+ QObject::connect(
+ ui->customWidget_coordinator , &Coordinator::agentIDsToCoordinateChanged ,
+ ui->customWidget_controller_tabs , &ControllerTabs::setAgentIDsToCoordinate
+ );
+
+ // CONNECT SIGNAL/SLOT FOR PASSING THE OBJECT NAME FOR WHICH
+ // POSE DATA SHOULD BE DISPLAYED
+ // This is passed from the "top banner" to the "controller tabs"
+ // because the "top banner" is in charge of fetching the object
+ // name from the database, and the "controller tabs" are where
+ // the pose data is displayed
+ QObject::connect(
+ ui->customWidget_topBanner , &TopBanner::objectNameForDisplayingPoseDataValueChanged ,
+ ui->customWidget_controller_tabs , &ControllerTabs::setObjectNameForDisplayingPoseData
+ );
+
+
+ // TOGGLE THE CONTROLLERS THAT ARE VISIBLE
+ // By default all controller buttons and tabs are visible
+ // and the menu item is checked. Hence, to hide a controller
+ // the menu item simply needs to be "triggered"
+
+ // > For the picker controller
+ ui->action_showHideController_picker->trigger();
+ // > For the tuning controller
+ ui->action_showHideController_tuning->trigger();
+ // > For the template controller
+ ui->action_showHideController_template->trigger();
+
+}
+
+MainWindow::~MainWindow()
+{
+ delete ui;
+}
+
+
+void MainWindow::on_actionShowHide_Coordinator_triggered()
+{
+ //ui->customWidget_enableControllerLoadYamlBar->setEnabled(false);
+ if ( ui->customWidget_coordinator->isHidden() )
+ {
+ ui->customWidget_coordinator->show();
+ ui->coordinator_to_main_panel_vertical_line->show();
+ QString qstr = "Hide Coordinator";
+ ui->actionShowHide_Coordinator->setText(qstr);
+ }
+ else
+ {
+ ui->customWidget_coordinator->hide();
+ ui->coordinator_to_main_panel_vertical_line->hide();
+ QString qstr = "Show Coordinator";
+ ui->actionShowHide_Coordinator->setText(qstr);
+ }
+}
+
+
+void MainWindow::on_action_LoadYAML_BatteryMonitor_triggered()
+{
+#ifdef CATKIN_MAKE
+ // Inform the user that the menu item was selected
+ ROS_INFO("[FLYING AGENT GUI MAIN WINDOW] Load Battery Monitor YAML was clicked.");
+
+ // Create a local variable for the message
+ StringWithHeader yaml_filename_msg;
+ // Specify the data
+ yaml_filename_msg.data = "BatteryMonitor";
+ // Set for whom this applies to
+ yaml_filename_msg.shouldCheckForAgentID = false;
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+#endif
+}
+
+
+void MainWindow::on_action_LoadYAML_FlyingAgentClientConfig_triggered()
+{
+#ifdef CATKIN_MAKE
+ // Inform the user that the menu item was selected
+ ROS_INFO("[FLYING AGENT GUI MAIN WINDOW] Load Client Config YAML was clicked.");
+
+ // Create a local variable for the message
+ StringWithHeader yaml_filename_msg;
+ // Specify the data
+ yaml_filename_msg.data = "FlyingAgentClientConfig";
+ // Set for whom this applies to
+ yaml_filename_msg.shouldCheckForAgentID = false;
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+#endif
+}
+
+
+
+
+
+void MainWindow::on_action_showHideController_default_changed()
+{
+ // Notify the UI elements of this change
+ ui->customWidget_enableControllerLoadYamlBar->showHideController_default_changed();
+ ui->customWidget_controller_tabs->showHideController_default_changed();
+}
+
+
+void MainWindow::on_action_showHideController_student_changed()
+{
+ // Notify the UI elements of this change
+ ui->customWidget_enableControllerLoadYamlBar->showHideController_student_changed();
+ ui->customWidget_controller_tabs->showHideController_student_changed();
+}
+
+
+void MainWindow::on_action_showHideController_picker_changed()
+{
+ // Notify the UI elements of this change
+ ui->customWidget_enableControllerLoadYamlBar->showHideController_picker_changed();
+ ui->customWidget_controller_tabs->showHideController_picker_changed();
+}
+
+void MainWindow::on_action_showHideController_tuning_changed()
+{
+ // Notify the UI elements of this change
+ ui->customWidget_enableControllerLoadYamlBar->showHideController_tuning_changed();
+ ui->customWidget_controller_tabs->showHideController_tuning_changed();
+}
+
+void MainWindow::on_action_showHideController_template_changed()
+{
+ // Notify the UI elements of this change
+ ui->customWidget_enableControllerLoadYamlBar->showHideController_template_changed();
+ ui->customWidget_controller_tabs->showHideController_template_changed();
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool MainWindow::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[ENABLE CONTROLLER LOAD YAML GUI BAR] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[ENABLE CONTROLLER LOAD YAML GUI BAR] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f90b07f4c9ded2fb6ef36296e73d9baf71489cc1
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
@@ -0,0 +1,2019 @@
+// Copyright (C) 2019, 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 GUI for the Picker Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "pickercontrollertab.h"
+#include "ui_pickercontrollertab.h"
+
+PickerControllerTab::PickerControllerTab(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::PickerControllerTab)
+{
+ ui->setupUi(this);
+
+
+ // HIDE ALL THE "GREEN FRAMES"
+ // > These indicate which state is currently active
+ ui->frame_goto_start_active->setVisible(true);
+ ui->frame_attach_active ->setVisible(true);
+ ui->frame_lift_up_active ->setVisible(true);
+ ui->frame_goto_end_active ->setVisible(true);
+ ui->frame_put_down_active ->setVisible(true);
+ ui->frame_squat_active ->setVisible(true);
+ ui->frame_jump_active ->setVisible(true);
+ ui->frame_standby_active ->setVisible(true);
+ // > Make them all white
+ ui->frame_goto_start_active->setStyleSheet("background-color:white;");
+ ui->frame_attach_active ->setStyleSheet("background-color:white;");
+ ui->frame_lift_up_active ->setStyleSheet("background-color:white;");
+ ui->frame_goto_end_active ->setStyleSheet("background-color:white;");
+ ui->frame_put_down_active ->setStyleSheet("background-color:white;");
+ ui->frame_squat_active ->setStyleSheet("background-color:white;");
+ ui->frame_jump_active ->setStyleSheet("background-color:white;");
+ ui->frame_standby_active ->setStyleSheet("background-color:white;");
+
+
+ // SET DEFAULTS FOR THE INCREMENT
+ ui->lineEdit_increment_x ->setText(QString::number( DEFAULT_INCREMENT_POSITION_XY, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_increment_y ->setText(QString::number( DEFAULT_INCREMENT_POSITION_XY, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_increment_z ->setText(QString::number( DEFAULT_INCREMENT_POSITION_Z, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_increment_yaw ->setText(QString::number( DEFAULT_INCREMENT_ANGLE_DEGREES, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ ui->lineEdit_increment_mass->setText(QString::number( DEFAULT_INCREMENT_MASS_GRAMS, 'f', DECIMAL_PLACES_MASS_GRAMS));
+
+ // SET THE INITIAL CHECKBOX STATE FOR SMOOTHING
+ ui->checkbox_goto_start->setChecked(true);
+ ui->checkbox_attach ->setChecked(true);
+ ui->checkbox_lift_up ->setChecked(true);
+ ui->checkbox_goto_end ->setChecked(true);
+ ui->checkbox_put_down ->setChecked(true);
+ ui->checkbox_squat ->setChecked(true);
+ ui->checkbox_jump ->setChecked(false);
+ ui->checkbox_standby ->setChecked(true);
+ ui->checkbox_current ->setChecked(false);
+
+ // SET THE INITIAL CHECKBOX STATE FOR PUBLISHING EVERY CHANGED VALUE
+ ui->checkbox_should_publish_value_changed->setChecked(true);
+
+ // HIDE ALL THE "RED FRAMES"
+ // > These indicate when an occuled measurement is recieved
+ ui->frame_x_unavailable ->setVisible(false);
+ ui->frame_y_unavailable ->setVisible(false);
+ ui->frame_z_unavailable ->setVisible(false);
+ ui->frame_yaw_unavailable->setVisible(false);
+
+ // SET DEFAULTS FOR ALL THE {x,y,z,yaw,mass} LineEdits
+ // > For Goto Start
+ ui->lineEdit_goto_start_x ->setText(QString::number( PICKER_DEFAULT_X, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_start_y ->setText(QString::number( PICKER_DEFAULT_Y, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_start_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_start_yaw->setText(QString::number( PICKER_DEFAULT_YAW_DEGREES, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // > For Attach
+ ui->lineEdit_attach_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ // > For Lift Up
+ ui->lineEdit_lift_up_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_lift_up_mass ->setText(QString::number( PICKER_DEFAULT_MASS_GRAMS, 'f', DECIMAL_PLACES_MASS_GRAMS));
+ // > For Goto End
+ ui->lineEdit_goto_end_x ->setText(QString::number( PICKER_DEFAULT_X, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_end_y ->setText(QString::number( PICKER_DEFAULT_Y, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_end_yaw ->setText(QString::number( PICKER_DEFAULT_YAW_DEGREES, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // > For Put Down
+ ui->lineEdit_put_down_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ // > For Squat
+ ui->lineEdit_squat_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ // > For Jump
+ ui->lineEdit_jump_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ // > For Standby
+ ui->lineEdit_standby_x ->setText(QString::number( PICKER_DEFAULT_X, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_standby_y ->setText(QString::number( PICKER_DEFAULT_Y, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_standby_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_standby_yaw ->setText(QString::number( PICKER_DEFAULT_YAW_DEGREES, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ ui->lineEdit_standby_mass ->setText(QString::number( PICKER_DEFAULT_MASS_GRAMS, 'f', DECIMAL_PLACES_MASS_GRAMS));
+
+
+
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[PICKER CONTROLLER TAB GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
+ requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("PickerControllerService/RequestSetpointChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("PickerControllerService/SetpointChanged", 1, &PickerControllerTab::setpointChangedCallback, this);
+ }
+
+ // GET THE CURRENT SETPOINT
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("PickerControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[PICKER CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+ }
+
+#endif
+}
+
+PickerControllerTab::~PickerControllerTab()
+{
+ delete ui;
+}
+
+
+
+
+
+
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+// PUBLISH CHANGES IN STATE AND SETPOINT
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+
+void PickerControllerTab::publish_setpoint_if_current_state_matches(QVector<int> state_to_match)
+{
+ m_current_picker_state_mutex.lock();
+ if ( state_to_match.contains(m_current_picker_state) )
+ {
+ publish_request_setpoint_change_for_state(m_current_picker_state);
+ }
+ m_current_picker_state_mutex.unlock();
+}
+
+void PickerControllerTab::publish_request_setpoint_change_for_state(int state_to_publish)
+{
+ // Initiliase variables for the setpoing
+ bool req_should_smooth;
+ float req_x;
+ float req_y;
+ float req_z;
+ float req_yaw;
+ float req_mass;
+
+
+ // Switch between the possible states and get the respectively
+ // values for the setpoint
+ switch (state_to_publish)
+ {
+ case PICKER_STATE_STANDBY:
+ {
+ req_should_smooth = ui->checkbox_standby->isChecked();
+ req_x = (ui->lineEdit_standby_x ->text()).toFloat();
+ req_y = (ui->lineEdit_standby_y ->text()).toFloat();
+ req_z = (ui->lineEdit_standby_z ->text()).toFloat();
+ req_yaw = (ui->lineEdit_standby_yaw ->text()).toFloat();
+ req_mass = (ui->lineEdit_standby_mass->text()).toFloat();
+ break;
+ }
+ case PICKER_STATE_GOTO_START:
+ {
+ req_should_smooth = ui->checkbox_goto_start->isChecked();
+ req_x = (ui->lineEdit_goto_start_x ->text()).toFloat();
+ req_y = (ui->lineEdit_goto_start_y ->text()).toFloat();
+ req_z = (ui->lineEdit_goto_start_z ->text()).toFloat();
+ req_yaw = (ui->lineEdit_goto_start_yaw->text()).toFloat();
+ req_mass = (ui->lineEdit_standby_mass ->text()).toFloat();
+ break;
+ }
+ case PICKER_STATE_ATTACH:
+ {
+ req_should_smooth = ui->checkbox_attach->isChecked();
+ req_x = (ui->lineEdit_goto_start_x ->text()).toFloat();
+ req_y = (ui->lineEdit_goto_start_y ->text()).toFloat();
+ req_z = (ui->lineEdit_attach_z ->text()).toFloat();
+ req_yaw = (ui->lineEdit_goto_start_yaw->text()).toFloat();
+ req_mass = (ui->lineEdit_standby_mass ->text()).toFloat();
+ break;
+ }
+ case PICKER_STATE_LIFT_UP:
+ {
+ req_should_smooth = ui->checkbox_lift_up->isChecked();
+ req_x = (ui->lineEdit_goto_start_x ->text()).toFloat();
+ req_y = (ui->lineEdit_goto_start_y ->text()).toFloat();
+ req_z = (ui->lineEdit_lift_up_z ->text()).toFloat();
+ req_yaw = (ui->lineEdit_goto_start_yaw->text()).toFloat();
+ req_mass = (ui->lineEdit_lift_up_mass ->text()).toFloat();
+ break;
+ }
+ case PICKER_STATE_GOTO_END:
+ {
+ req_should_smooth = ui->checkbox_goto_end->isChecked();
+ req_x = (ui->lineEdit_goto_end_x ->text()).toFloat();
+ req_y = (ui->lineEdit_goto_end_y ->text()).toFloat();
+ req_z = (ui->lineEdit_lift_up_z ->text()).toFloat();
+ req_yaw = (ui->lineEdit_goto_end_yaw->text()).toFloat();
+ req_mass = (ui->lineEdit_lift_up_mass->text()).toFloat();
+ break;
+ }
+ case PICKER_STATE_PUT_DOWN:
+ {
+ req_should_smooth = ui->checkbox_put_down->isChecked();
+ req_x = (ui->lineEdit_goto_end_x ->text()).toFloat();
+ req_y = (ui->lineEdit_goto_end_y ->text()).toFloat();
+ req_z = (ui->lineEdit_put_down_z ->text()).toFloat();
+ req_yaw = (ui->lineEdit_goto_end_yaw->text()).toFloat();
+ req_mass = (ui->lineEdit_lift_up_mass->text()).toFloat();
+ break;
+ }
+ case PICKER_STATE_SQUAT:
+ {
+ req_should_smooth = ui->checkbox_squat->isChecked();
+ req_x = (ui->lineEdit_goto_end_x ->text()).toFloat();
+ req_y = (ui->lineEdit_goto_end_y ->text()).toFloat();
+ req_z = (ui->lineEdit_squat_z ->text()).toFloat();
+ req_yaw = (ui->lineEdit_goto_end_yaw->text()).toFloat();
+ req_mass = (ui->lineEdit_standby_mass->text()).toFloat();
+ break;
+ }
+ case PICKER_STATE_JUMP:
+ {
+ req_should_smooth = ui->checkbox_jump->isChecked();
+ req_x = (ui->lineEdit_goto_end_x ->text()).toFloat();
+ req_y = (ui->lineEdit_goto_end_y ->text()).toFloat();
+ req_z = (ui->lineEdit_jump_z ->text()).toFloat();
+ req_yaw = (ui->lineEdit_goto_end_yaw->text()).toFloat();
+ req_mass = (ui->lineEdit_standby_mass->text()).toFloat();
+ break;
+ }
+ default:
+ {
+ req_should_smooth = true;
+ req_x = PICKER_DEFAULT_X;
+ req_y = PICKER_DEFAULT_Y;
+ req_z = PICKER_DEFAULT_Z;
+ req_yaw = PICKER_DEFAULT_YAW_DEGREES;
+ req_mass = PICKER_DEFAULT_MASS_GRAMS;
+ break;
+ }
+ }
+
+ // Publish a ROS message with the setpoint to be requested
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw,mass) values
+ msg.x = req_x;
+ msg.y = req_y;
+ msg.z = req_z;
+ msg.yaw = req_yaw * DEG2RAD;
+ msg.mass = req_mass;
+
+ // Fill in the "should smooth" value
+ msg.isChecked = req_should_smooth;
+
+ // Fill in the "picker state" value
+ msg.buttonID = state_to_publish;
+
+ // Publish the setpoint
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[PICKER CONTROLLER GUI] Published request for setpoint change to: [" << req_x << ", "<< req_y << ", "<< req_z << ", "<< req_yaw << "], with {mass,smooth,state} = " << req_mass << ", "<< req_should_smooth << ", "<< state_to_publish );
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[PICKER CONTROLLER GUI] would publish request for: [" << req_x << ", "<< req_y << ", "<< req_z << ", "<< req_yaw << "], with {mass,smooth,state} = " << req_mass << ", "<< req_should_smooth << ", "<< state_to_publish;
+#endif
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+//
+// CCCC H H A N N GGGG EEEEE DDDD
+// C H H A A NN N G E D D
+// C HHHHH A A N N N G EEE D D
+// C H H AAAAA N NN G G E D D
+// CCCC H H A A N N GGGG EEEEE DDDD
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void PickerControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
+{
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+
+ // EXTRACT THE SETPOINT
+ float x = newSetpoint.x;
+ float y = newSetpoint.y;
+ float z = newSetpoint.z;
+ float yaw = newSetpoint.yaw;
+
+ float mass = newSetpoint.yaw;
+ bool smooth = newSetpoint.isChecked;
+ int state = newSetpoint.buttonID;
+
+ // UPDATE THE SETPOINT LineEdits
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_current_x->setText(qstr + QString::number( x, 'f', DECIMAL_PLACES_POSITION));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_current_y->setText(qstr + QString::number( y, 'f', DECIMAL_PLACES_POSITION));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_current_z->setText(qstr + QString::number( z, 'f', DECIMAL_PLACES_POSITION));
+
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+
+ ui->lineEdit_current_mass->setText(qstr + QString::number( mass, 'f', DECIMAL_PLACES_MASS_GRAMS));
+
+ ui->checkbox_current->setChecked(smooth);
+
+
+
+ // MAKE THE "GREEN FRAME" VISIBLE ONLY FOR THE CURRENT STATE
+ // Lock the mutex that we will access "m_current_picker_state"
+ m_current_picker_state_mutex.lock();
+
+ // Put things into more readable variables
+ int new_state = state;
+ int previous_state = m_current_picker_state;
+
+ // Update the class variable for tracking the state
+ if ( new_state != previous_state )
+ m_current_picker_state = new_state;
+
+ // Change the "GREEN FRAME" if the state changed
+ if ( new_state != previous_state )
+ {
+ // Make the new one green
+ switch (new_state)
+ {
+ case PICKER_STATE_STANDBY:
+ {
+ ui->frame_standby_active->setStyleSheet("background-color:green;");
+ break;
+ }
+ case PICKER_STATE_GOTO_START:
+ {
+ ui->frame_goto_start_active->setStyleSheet("background-color:green;");
+ break;
+ }
+ case PICKER_STATE_ATTACH:
+ {
+ ui->frame_attach_active->setStyleSheet("background-color:green;");
+ break;
+ }
+ case PICKER_STATE_LIFT_UP:
+ {
+ ui->frame_lift_up_active->setStyleSheet("background-color:green;");
+ break;
+ }
+ case PICKER_STATE_GOTO_END:
+ {
+ ui->frame_goto_end_active->setStyleSheet("background-color:green;");
+ break;
+ }
+ case PICKER_STATE_PUT_DOWN:
+ {
+ ui->frame_put_down_active->setStyleSheet("background-color:green;");
+ break;
+ }
+ case PICKER_STATE_SQUAT:
+ {
+ ui->frame_squat_active->setStyleSheet("background-color:green;");
+ break;
+ }
+ case PICKER_STATE_JUMP:
+ {
+ ui->frame_jump_active->setStyleSheet("background-color:green;");
+ break;
+ }
+ default:
+ {
+ break;
+ }
+ }
+
+ // And the old one not white
+ switch (previous_state)
+ {
+ case PICKER_STATE_STANDBY:
+ {
+ ui->frame_standby_active->setStyleSheet("background-color:white;");
+ break;
+ }
+ case PICKER_STATE_GOTO_START:
+ {
+ ui->frame_goto_start_active->setStyleSheet("background-color:white;");
+ break;
+ }
+ case PICKER_STATE_ATTACH:
+ {
+ ui->frame_attach_active->setStyleSheet("background-color:white;");
+ break;
+ }
+ case PICKER_STATE_LIFT_UP:
+ {
+ ui->frame_lift_up_active->setStyleSheet("background-color:white;");
+ break;
+ }
+ case PICKER_STATE_GOTO_END:
+ {
+ ui->frame_goto_end_active->setStyleSheet("background-color:white;");
+ break;
+ }
+ case PICKER_STATE_PUT_DOWN:
+ {
+ ui->frame_put_down_active->setStyleSheet("background-color:white;");
+ break;
+ }
+ case PICKER_STATE_SQUAT:
+ {
+ ui->frame_squat_active->setStyleSheet("background-color:white;");
+ break;
+ }
+ case PICKER_STATE_JUMP:
+ {
+ ui->frame_jump_active->setStyleSheet("background-color:white;");
+ break;
+ }
+ default:
+ {
+ break;
+ }
+ }
+ }
+ else
+ {
+ // NOTHING TO CHANGE
+ }
+ // Unlock the mutex for accessing "m_current_picker_state"
+ m_current_picker_state_mutex.unlock();
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// PPPP OOO SSSS EEEEE DDDD A TTTTT A
+// P P O O S E D D A A T A A
+// PPPP O O SSS EEE D D A A T A A
+// P O O S E D D AAAAA T AAAAA
+// P OOO SSSS EEEEE DDDD A A T A A
+// ----------------------------------------------------------------------------------
+
+
+void PickerControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+{
+ if (!occluded)
+ {
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+ // UPDATE THE MEASUREMENT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_x->setText(qstr + QString::number( x, 'f', DECIMAL_PLACES_POSITION));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_y->setText(qstr + QString::number( y, 'f', DECIMAL_PLACES_POSITION));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', DECIMAL_PLACES_POSITION));
+
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+
+ // GET THE CURRENT SETPOINT ERROR
+ float error_x = x - (ui->lineEdit_current_x->text() ).toFloat();
+ float error_y = y - (ui->lineEdit_current_y->text() ).toFloat();
+ float error_z = z - (ui->lineEdit_current_z->text() ).toFloat();
+
+ float error_yaw_deg = yaw * RAD2DEG - (ui->lineEdit_current_yaw->text()).toFloat();
+
+ // UPDATE THE ERROR COLUMN
+ if (error_x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_x->setText(qstr + QString::number( error_x, 'f', DECIMAL_PLACES_POSITION));
+ if (error_y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_y->setText(qstr + QString::number( error_y, 'f', DECIMAL_PLACES_POSITION));
+ if (error_z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', DECIMAL_PLACES_POSITION));
+
+ if (error_yaw_deg < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw_deg , 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+
+ // Ensure the red frames are not visible
+ if ( ui->frame_x_unavailable->isVisible() )
+ {
+ ui->frame_x_unavailable ->setVisible(false);
+ ui->frame_y_unavailable ->setVisible(false);
+ ui->frame_z_unavailable ->setVisible(false);
+ ui->frame_yaw_unavailable->setVisible(false);
+ }
+ }
+ else
+ {
+ // Make visible the red frames to indicate occluded
+ if ( !(ui->frame_x_unavailable->isVisible()) )
+ {
+ ui->frame_x_unavailable ->setVisible(true);
+ ui->frame_y_unavailable ->setVisible(true);
+ ui->frame_z_unavailable ->setVisible(true);
+ ui->frame_yaw_unavailable->setVisible(true);
+ }
+ }
+}
+
+
+void PickerControllerTab::poseDataUnavailableSlot()
+{
+ ui->lineEdit_measured_x->setText("xx.xx");
+ ui->lineEdit_measured_y->setText("xx.xx");
+ ui->lineEdit_measured_z->setText("xx.xx");
+
+ ui->lineEdit_measured_yaw->setText("xx.xx");
+
+ ui->lineEdit_error_x->setText("xx.xx");
+ ui->lineEdit_error_y->setText("xx.xx");
+ ui->lineEdit_error_z->setText("xx.xx");
+ ui->lineEdit_error_yaw->setText("xx.xx");
+}
+
+
+
+
+
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+// STATE BUTTONS CLICKED
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+
+void PickerControllerTab::on_button_goto_start_clicked()
+{
+ // Directly call the function that publishes state (and setpoint) changes
+ publish_request_setpoint_change_for_state(PICKER_STATE_GOTO_START);
+}
+
+void PickerControllerTab::on_button_attach_clicked()
+{
+ // Directly call the function that publishes state (and setpoint) changes
+ publish_request_setpoint_change_for_state(PICKER_STATE_ATTACH);
+}
+
+void PickerControllerTab::on_button_lift_up_clicked()
+{
+ // Directly call the function that publishes state (and setpoint) changes
+ publish_request_setpoint_change_for_state(PICKER_STATE_LIFT_UP);
+}
+
+void PickerControllerTab::on_button_goto_end_clicked()
+{
+ // Directly call the function that publishes state (and setpoint) changes
+ publish_request_setpoint_change_for_state(PICKER_STATE_GOTO_END);
+}
+
+void PickerControllerTab::on_button_put_down_clicked()
+{
+ // Directly call the function that publishes state (and setpoint) changes
+ publish_request_setpoint_change_for_state(PICKER_STATE_PUT_DOWN);
+}
+
+void PickerControllerTab::on_button_squat_clicked()
+{
+ // Directly call the function that publishes state (and setpoint) changes
+ publish_request_setpoint_change_for_state(PICKER_STATE_SQUAT);
+}
+
+void PickerControllerTab::on_button_jump_clicked()
+{
+ // Directly call the function that publishes state (and setpoint) changes
+ publish_request_setpoint_change_for_state(PICKER_STATE_JUMP);
+}
+
+void PickerControllerTab::on_button_standby_clicked()
+{
+ // Directly call the function that publishes state (and setpoint) changes
+ publish_request_setpoint_change_for_state(PICKER_STATE_STANDBY);
+}
+
+
+
+
+
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+// CHECK BOXES CLICKED
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+
+void PickerControllerTab::on_checkbox_goto_start_clicked()
+{
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_checkbox_attach_clicked()
+{
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_checkbox_lift_up_clicked()
+{
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_checkbox_goto_end_clicked()
+{
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_checkbox_put_down_clicked()
+{
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_checkbox_squat_clicked()
+{
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_checkbox_jump_clicked()
+{
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_checkbox_standby_clicked()
+{
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+
+
+
+
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+// INCREMENT BUTTONS CLICKED
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+
+// >> FOR GOTO START
+
+void PickerControllerTab::on_button_goto_start_inc_minus_x_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_start_x->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_x ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_goto_start_x->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_start_inc_plus_x_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_start_x->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_x ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_goto_start_x->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_start_inc_minus_y_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_start_y->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_y ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_goto_start_y->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_start_inc_plus_y_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_start_y->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_y ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_goto_start_y->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_start_inc_minus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_start_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_goto_start_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_start_inc_plus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_start_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_goto_start_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_start_inc_minus_yaw_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_start_yaw->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_yaw ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_goto_start_yaw->setText(QString::number( new_value, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_start_inc_plus_yaw_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_start_yaw->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_yaw ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_goto_start_yaw->setText(QString::number( new_value, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR ATTACH
+
+void PickerControllerTab::on_button_attach_inc_minus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_attach_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_attach_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_attach_inc_plus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_attach_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_attach_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR LIFT UP
+
+void PickerControllerTab::on_button_lift_up_inc_minus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_lift_up_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_lift_up_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_lift_up_inc_plus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_lift_up_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_lift_up_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_lift_up_inc_minus_mass_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_lift_up_mass->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_mass ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_lift_up_mass->setText(QString::number( new_value, 'f', DECIMAL_PLACES_MASS_GRAMS));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_lift_up_inc_plus_mass_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_lift_up_mass->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_mass ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_lift_up_mass->setText(QString::number( new_value, 'f', DECIMAL_PLACES_MASS_GRAMS));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR GOTO END
+
+void PickerControllerTab::on_button_goto_end_inc_minus_x_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_end_x->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_x ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_goto_end_x->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_end_inc_plus_x_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_end_x->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_x ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_goto_end_x->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_end_inc_minus_y_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_end_y->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_y ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_goto_end_y->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_end_inc_plus_y_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_end_y->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_y ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_goto_end_y->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_end_inc_minus_yaw_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_end_yaw->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_yaw ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_goto_end_yaw->setText(QString::number( new_value, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_goto_end_inc_plus_yaw_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_goto_end_yaw->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_yaw ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_goto_end_yaw->setText(QString::number( new_value, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR PUT DOWN
+
+void PickerControllerTab::on_button_put_down_inc_minus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_put_down_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_put_down_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_put_down_inc_plus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_put_down_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_put_down_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR SQUAT
+
+void PickerControllerTab::on_button_squat_inc_minus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_squat_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_squat_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_squat_inc_plus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_squat_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_squat_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR JUMP
+
+void PickerControllerTab::on_button_jump_inc_minus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_jump_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_jump_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_jump_inc_plus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_jump_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_jump_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR STANDBY
+
+void PickerControllerTab::on_button_stanby_inc_minus_x_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_x->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_x ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_standby_x->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_stanby_inc_plus_x_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_x->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_x ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_standby_x->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_stanby_inc_minus_y_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_y->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_y ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_standby_y->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_stanby_inc_plus_y_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_y->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_y ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_standby_y->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_stanby_inc_minus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_standby_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_stanby_inc_plus_z_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_z->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_z ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_standby_z->setText(QString::number( new_value, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_stanby_inc_minus_yaw_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_yaw->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_yaw ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_standby_yaw->setText(QString::number( new_value, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_stanby_inc_plus_yaw_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_yaw->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_yaw ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_standby_yaw->setText(QString::number( new_value, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_stanby_inc_minus_mass_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_mass->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_mass ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value - current_inc;
+ ui->lineEdit_standby_mass->setText(QString::number( new_value, 'f', DECIMAL_PLACES_MASS_GRAMS));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_button_stanby_inc_plus_mass_clicked()
+{
+ // Get the value from the respective line edit and increment
+ float current_value = (ui->lineEdit_standby_mass->text()).toFloat();
+ float current_inc = (ui->lineEdit_increment_mass ->text()).toFloat();
+ // Compute the new value and set it back
+ float new_value = current_value + current_inc;
+ ui->lineEdit_standby_mass->setText(QString::number( new_value, 'f', DECIMAL_PLACES_MASS_GRAMS));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+
+
+
+
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+// LINE EDITS FINISHED EDITING
+// ---------------------------------------------------------- //
+// ---------------------------------------------------------- //
+
+// >> FOR GOTO START
+
+void PickerControllerTab::on_lineEdit_goto_start_x_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_goto_start_x->text()).toFloat();
+ ui->lineEdit_goto_start_x->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_goto_start_y_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_goto_start_y->text()).toFloat();
+ ui->lineEdit_goto_start_y->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_goto_start_z_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_goto_start_z->text()).toFloat();
+ ui->lineEdit_goto_start_z->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_goto_start_yaw_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_goto_start_yaw->text()).toFloat();
+ ui->lineEdit_goto_start_yaw->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR ATTACH
+
+void PickerControllerTab::on_lineEdit_attach_z_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_attach_z->text()).toFloat();
+ ui->lineEdit_attach_z->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR LIFT UP
+
+void PickerControllerTab::on_lineEdit_lift_up_z_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_lift_up_z->text()).toFloat();
+ ui->lineEdit_lift_up_z->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_lift_up_mass_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_lift_up_mass->text()).toFloat();
+ ui->lineEdit_lift_up_mass->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_MASS_GRAMS));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR GOTO END
+
+void PickerControllerTab::on_lineEdit_goto_end_x_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_goto_end_x->text()).toFloat();
+ ui->lineEdit_goto_end_x->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_goto_end_y_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_goto_end_y->text()).toFloat();
+ ui->lineEdit_goto_end_y->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_goto_end_yaw_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_goto_end_yaw->text()).toFloat();
+ ui->lineEdit_goto_end_yaw->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR PUT DOWN
+
+void PickerControllerTab::on_lineEdit_put_down_z_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_put_down_z->text()).toFloat();
+ ui->lineEdit_put_down_z->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR SQUAT
+
+void PickerControllerTab::on_lineEdit_squat_z_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_squat_z->text()).toFloat();
+ ui->lineEdit_squat_z->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR JUMP
+
+void PickerControllerTab::on_lineEdit_jump_z_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_jump_z->text()).toFloat();
+ ui->lineEdit_jump_z->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR STANDBY
+
+void PickerControllerTab::on_lineEdit_standby_x_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_standby_x->text()).toFloat();
+ ui->lineEdit_standby_x->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_standby_y_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_standby_y->text()).toFloat();
+ ui->lineEdit_standby_y->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_standby_z_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_standby_z->text()).toFloat();
+ ui->lineEdit_standby_z->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_standby_yaw_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_standby_yaw->text()).toFloat();
+ ui->lineEdit_standby_yaw->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+void PickerControllerTab::on_lineEdit_standby_mass_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_standby_mass->text()).toFloat();
+ ui->lineEdit_standby_mass->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_MASS_GRAMS));
+ // Call the function to publish the new setpoint if appropriate
+ if (ui->checkbox_should_publish_value_changed->isChecked())
+ {
+ QVector<int> states_for_which_this_change_applies;
+ states_for_which_this_change_applies.append(PICKER_STATE_STANDBY);
+ states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
+ states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
+ states_for_which_this_change_applies.append(PICKER_STATE_SQUAT);
+ states_for_which_this_change_applies.append(PICKER_STATE_JUMP);
+ publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
+ }
+}
+
+// >> FOR INCREMENTS
+
+void PickerControllerTab::on_lineEdit_increment_x_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_increment_x->text()).toFloat();
+ ui->lineEdit_increment_x->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+}
+
+void PickerControllerTab::on_lineEdit_increment_y_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_increment_y->text()).toFloat();
+ ui->lineEdit_increment_y->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+}
+
+void PickerControllerTab::on_lineEdit_increment_z_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_increment_z->text()).toFloat();
+ ui->lineEdit_increment_z->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_POSITION));
+}
+
+void PickerControllerTab::on_lineEdit_increment_yaw_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_increment_yaw->text()).toFloat();
+ ui->lineEdit_increment_yaw->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+}
+
+void PickerControllerTab::on_lineEdit_increment_mass_editingFinished()
+{
+ // Get the value and set it back with two decimal places
+ float value_entered = (ui->lineEdit_increment_mass->text()).toFloat();
+ ui->lineEdit_increment_mass->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_MASS_GRAMS));
+}
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void PickerControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+ // // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("PickerControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[PICKER CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+
+ // SUBSCRIBERS
+ // > For receiving message that the setpoint was changed
+ setpointChangedSubscriber = agent_base_nodeHandle.subscribe("PickerControllerService/SetpointChanged", 1, &PickerControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // Set information back to the default
+ ui->lineEdit_current_x ->setText("xx.xx");
+ ui->lineEdit_current_y ->setText("xx.xx");
+ ui->lineEdit_current_z ->setText("xx.xx");
+ ui->lineEdit_current_yaw ->setText("xx.xx");
+ ui->lineEdit_current_mass->setText("xx.xx");
+
+ }
+#endif
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
+// MM MM S G G H H E A A D D E R R
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void PickerControllerTab::fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool PickerControllerTab::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[PICKER CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PICKER CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PICKER CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp
similarity index 92%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp
index 63d73e661bc90c41a6090ed106e9292b1b1c8ae3..161b03ac9dad95a0aff307511e02adc9d18d9a3a 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
@@ -30,13 +30,16 @@
// ----------------------------------------------------------------------------------
+
+
+
#include "rosNodeThread_for_flyingAgentGUI.h"
-// #include "d_fall_pps/CMRead.h"
-// #include "d_fall_pps/CMUpdate.h"
-// #include "d_fall_pps/CMCommand.h"
+// #include "dfall_pkg/CMRead.h"
+// #include "dfall_pkg/CMUpdate.h"
+// #include "dfall_pkg/CMCommand.h"
-// using namespace d_fall_pps;
+// using namespace dfall_pkg;
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name, QObject* parent)
@@ -65,7 +68,9 @@ bool rosNodeThread::init()
this->moveToThread(m_pThread); // QObject method
connect(m_pThread, SIGNAL(started()), this, SLOT(run()));
- ros::init(m_Init_argc, m_pInit_argv, m_node_name); // flyingAgentGUI is the name of this node
+ ros::init(m_Init_argc, m_pInit_argv, m_node_name);
+ // Note that the variable "m_node_name" should be the
+ // string "FlyingAgentGUI" in this case
if (!ros::master::check())
{
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/safecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/safecontrollertab.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/safecontrollertab.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/safecontrollertab.cpp
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..440daaac14f06788852f06f82dad300ce0294510
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
@@ -0,0 +1,944 @@
+// Copyright (C) 2019, 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 GUI for the Student Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "studentcontrollertab.h"
+#include "ui_studentcontrollertab.h"
+
+StudentControllerTab::StudentControllerTab(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::StudentControllerTab)
+{
+ ui->setupUi(this);
+
+ // Hide the two red frames that are used to indcated
+ // when pose data is occluded
+ ui->red_frame_position_left->setVisible(false);
+ ui->red_frame_position_right->setVisible(false);
+
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[STUDENT CONTROLLER TAB GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[STUDENT CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
+ requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("StudentControllerService/RequestSetpointChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("StudentControllerService/SetpointChanged", 1, &StudentControllerTab::setpointChangedCallback, this);
+ }
+
+ // CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
+ customButtonPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::CustomButtonWithHeader>("StudentControllerService/CustomButtonPressed", 1);
+
+ // GET THE CURRENT SETPOINT
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("StudentControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[STUDENT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+ }
+
+#endif
+
+}
+
+StudentControllerTab::~StudentControllerTab()
+{
+ delete ui;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+//
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void StudentControllerTab::publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer)
+{
+ // Initialise the message as a local variable
+ dfall_pkg::CustomButtonWithHeader msg;
+ // Fill the header of the message
+ fillCustomButtonMessageHeader( msg );
+ // Fill in the button index
+ msg.button_index = button_index;
+ // Get the line edit data, as a float if possible
+ bool isValidFloat = false;
+ float lineEdit_as_float = (lineEdit_pointer->text()).toFloat(&isValidFloat);
+ // Fill in the data
+ if (isValidFloat)
+ msg.float_data = lineEdit_as_float;
+ else
+ msg.string_data = (lineEdit_pointer->text()).toStdString();
+ // Publish the setpoint
+ this->customButtonPublisher.publish(msg);
+ // Inform the user about the change
+ ROS_INFO_STREAM("[STUDENT CONTROLLER TAB GUI] button " << button_index << " clicked.");
+}
+#endif
+
+
+void StudentControllerTab::on_custom_button_1_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(1,ui->lineEdit_custom_1);
+#endif
+}
+
+void StudentControllerTab::on_custom_button_2_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(2,ui->lineEdit_custom_2);
+#endif
+}
+
+void StudentControllerTab::on_custom_button_3_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(3,ui->lineEdit_custom_3);
+#endif
+}
+
+void StudentControllerTab::on_custom_button_4_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(4,ui->lineEdit_custom_4);
+#endif
+}
+
+void StudentControllerTab::on_custom_button_5_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(5,ui->lineEdit_custom_5);
+#endif
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// PPPP OOO SSSS EEEEE DDDD A TTTTT A
+// P P O O S E D D A A T A A
+// PPPP O O SSS EEE D D A A T A A
+// P O O S E D D AAAAA T AAAAA
+// P OOO SSSS EEEEE DDDD A A T A A
+// ----------------------------------------------------------------------------------
+
+
+void StudentControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+{
+ if (!occluded)
+ {
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+ // UPDATE THE MEASUREMENT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_x->setText(qstr + QString::number( x, 'f', 3));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_y->setText(qstr + QString::number( y, 'f', 3));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', 3));
+
+ if (roll < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_roll->setText(qstr + QString::number( roll * RAD2DEG, 'f', 1));
+ if (pitch < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_pitch->setText(qstr + QString::number( pitch * RAD2DEG, 'f', 1));
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+
+ // GET THE CURRENT SETPOINT ERROR
+ float error_x = x - (ui->lineEdit_setpoint_current_x->text() ).toFloat();
+ float error_y = y - (ui->lineEdit_setpoint_current_y->text() ).toFloat();
+ float error_z = z - (ui->lineEdit_setpoint_current_z->text() ).toFloat();
+
+ float error_yaw_deg = yaw * RAD2DEG - (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+ // UPDATE THE ERROR COLUMN
+ if (error_x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_x->setText(qstr + QString::number( error_x, 'f', 3));
+ if (error_y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_y->setText(qstr + QString::number( error_y, 'f', 3));
+ if (error_z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', 3));
+
+ if (error_yaw_deg < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw_deg , 'f', 1));
+
+ // Ensure the red frames are not visible
+ if ( ui->red_frame_position_left->isVisible() )
+ ui->red_frame_position_left->setVisible(false);
+ if ( ui->red_frame_position_right->isVisible() )
+ ui->red_frame_position_right->setVisible(false);
+ }
+ else
+ {
+ // Make visible the red frames to indicate occluded
+ if ( !(ui->red_frame_position_left->isVisible()) )
+ ui->red_frame_position_left->setVisible(true);
+ if ( !(ui->red_frame_position_right->isVisible()) )
+ ui->red_frame_position_right->setVisible(true);
+ }
+}
+
+
+void StudentControllerTab::poseDataUnavailableSlot()
+{
+ ui->lineEdit_measured_x->setText("xx.xx");
+ ui->lineEdit_measured_y->setText("xx.xx");
+ ui->lineEdit_measured_z->setText("xx.xx");
+
+ ui->lineEdit_measured_roll->setText("xx.xx");
+ ui->lineEdit_measured_pitch->setText("xx.xx");
+ ui->lineEdit_measured_yaw->setText("xx.xx");
+
+ ui->lineEdit_error_x->setText("xx.xx");
+ ui->lineEdit_error_y->setText("xx.xx");
+ ui->lineEdit_error_z->setText("xx.xx");
+ ui->lineEdit_error_yaw->setText("xx.xx");
+
+
+}
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+//
+// CCCC H H A N N GGGG EEEEE DDDD
+// C H H A A NN N G E D D
+// C HHHHH A A N N N G EEE D D
+// C H H AAAAA N NN G G E D D
+// CCCC H H A A N N GGGG EEEEE DDDD
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void StudentControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
+{
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+
+ // EXTRACT THE SETPOINT
+ float x = newSetpoint.x;
+ float y = newSetpoint.y;
+ float z = newSetpoint.z;
+ float yaw = newSetpoint.yaw;
+
+ // UPDATE THE SETPOINT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_x->setText(qstr + QString::number( x, 'f', 3));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_y->setText(qstr + QString::number( y, 'f', 3));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_z->setText(qstr + QString::number( z, 'f', 3));
+
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+}
+#endif
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE QQQ U U EEEEE SSSS TTTTT N N EEEEE W W
+// R R E Q Q U U E S T NN N E W W
+// RRRR EEE Q Q U U EEE SSS T N N N EEE W W
+// R R E Q Q U U E S T N NN E W W W
+// R R EEEEE QQ Q UUU EEEEE SSSS T N N EEEEE W W
+//
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+// ----------------------------------------------------------------------------------
+
+
+void StudentControllerTab::publishSetpoint(float x, float y, float z, float yaw_degrees)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw_degrees * DEG2RAD;
+
+ // Publish the setpoint
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Published request for setpoint change to: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[STUDENT CONTROLLER GUI] would publish request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]";
+#endif
+}
+
+
+
+void StudentControllerTab::on_lineEdit_setpoint_new_x_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void StudentControllerTab::on_lineEdit_setpoint_new_y_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void StudentControllerTab::on_lineEdit_setpoint_new_z_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void StudentControllerTab::on_lineEdit_setpoint_new_yaw_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void StudentControllerTab::on_set_setpoint_button_clicked()
+{
+
+ // Initialise local variable for each of (x,y,z,yaw)
+ float x = 0.0f, y = 0.0f, z = 0.0f, yaw = 0.0f;
+
+ // Take the new value if available, otherwise use the old value
+ // > For x
+ if(!ui->lineEdit_setpoint_new_x->text().isEmpty())
+ x = (ui->lineEdit_setpoint_new_x->text()).toFloat();
+ else
+ x = (ui->lineEdit_setpoint_current_x->text()).toFloat();
+ // > For y
+ if(!ui->lineEdit_setpoint_new_y->text().isEmpty())
+ y = (ui->lineEdit_setpoint_new_y->text()).toFloat();
+ else
+ y = (ui->lineEdit_setpoint_current_y->text()).toFloat();
+ // > For z
+ if(!ui->lineEdit_setpoint_new_z->text().isEmpty())
+ z = (ui->lineEdit_setpoint_new_z->text()).toFloat();
+ else
+ z = (ui->lineEdit_setpoint_current_z->text()).toFloat();
+ // > For yaws
+ if(!ui->lineEdit_setpoint_new_yaw->text().isEmpty())
+ yaw = (ui->lineEdit_setpoint_new_yaw->text()).toFloat();
+ else
+ yaw = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+ // Call the function to publish the setpoint
+ publishSetpoint(x,y,z,yaw);
+}
+
+void StudentControllerTab::on_default_setpoint_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Publish this as a blank setpoint with the
+ // "buttonID" field set appropriately
+
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.buttonID = REQUEST_DEFAULT_SETPOINT_BUTTON_ID;
+
+ // Publish the default setpoint button press
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Published request for setpoint change to the default");
+#endif
+}
+
+void StudentControllerTab::on_x_increment_plus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_x->text().isEmpty())
+ {
+ increment_setpoint_by( (ui->lineEdit_setpoint_increment_x->text()).toFloat() ,0.0,0.0,0.0);
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[Student CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+
+ }
+}
+
+void StudentControllerTab::on_x_increment_minus_button_clicked()
+{
+
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_x->text().isEmpty())
+ {
+ increment_setpoint_by( -(ui->lineEdit_setpoint_increment_x->text()).toFloat() ,0.0,0.0,0.0);
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void StudentControllerTab::on_y_increment_plus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_y->text().isEmpty())
+ {
+ increment_setpoint_by(0.0, (ui->lineEdit_setpoint_increment_y->text()).toFloat() ,0.0,0.0);
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void StudentControllerTab::on_y_increment_minus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_y->text().isEmpty())
+ {
+ increment_setpoint_by(0.0, -(ui->lineEdit_setpoint_increment_y->text()).toFloat() ,0.0,0.0);
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment y setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void StudentControllerTab::on_z_increment_plus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_z->text().isEmpty())
+ {
+ increment_setpoint_by(0.0,0.0, (ui->lineEdit_setpoint_increment_z->text()).toFloat() ,0.0);
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment z setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void StudentControllerTab::on_z_increment_minus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_z->text().isEmpty())
+ {
+ // Call the function to increment the setpoint
+ increment_setpoint_by(0.0,0.0, -(ui->lineEdit_setpoint_increment_z->text()).toFloat() ,0.0);
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment z setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+void StudentControllerTab::on_yaw_increment_plus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
+ {
+ // Call the function to increment the setpoint
+ increment_setpoint_by(0.0,0.0,0.0, (ui->lineEdit_setpoint_increment_yaw->text()).toFloat() );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment yaw setpoint clicked but field is empty");
+ #endif
+ }
+}
+void StudentControllerTab::on_yaw_increment_minus_button_clicked()
+{
+ // Only need to do something if the field is not empty
+ if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
+ {
+ // Call the function to increment the setpoint
+ increment_setpoint_by(0.0,0.0,0.0, -(ui->lineEdit_setpoint_increment_yaw->text()).toFloat() );
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment yaw setpoint clicked but field is empty");
+ #endif
+ }
+}
+
+
+void StudentControllerTab::increment_setpoint_by(float x_inc, float y_inc, float z_inc, float yaw_inc_degrees)
+{
+
+ if (m_type == TYPE_AGENT)
+ {
+ // WHEN GUI IS IN AGENT TYPE MODE:
+ // APPLY THE INCREMENT FROM THE CURRENT SETPOINT
+ // Call the function to publish the setpoint
+ publishSetpoint(
+ (ui->lineEdit_setpoint_current_x->text() ).toFloat() + x_inc,
+ (ui->lineEdit_setpoint_current_y->text() ).toFloat() + y_inc,
+ (ui->lineEdit_setpoint_current_z->text() ).toFloat() + z_inc,
+ (ui->lineEdit_setpoint_current_yaw->text()).toFloat() + yaw_inc_degrees
+ );
+ }
+ else if (m_type == TYPE_COORDINATOR)
+ {
+ // WHEN GUI IS IN COORDINATOR TYPE MODE:
+ // APPLY THE INCREMENT FROM THE CURRENT SETPOINT
+
+ // Initialise local variable for each of (x,y,z,yaw)
+ float x = 0.0f, y = 0.0f, z = 0.4f, yaw = 0.0f;
+
+ // Take the new value if available, otherwise use the old value
+ // > For x
+ if(!ui->lineEdit_setpoint_new_x->text().isEmpty())
+ x = (ui->lineEdit_setpoint_new_x->text()).toFloat();
+ // > For x
+ if(!ui->lineEdit_setpoint_new_y->text().isEmpty())
+ y = (ui->lineEdit_setpoint_new_y->text()).toFloat();
+ // > For x
+ if(!ui->lineEdit_setpoint_new_z->text().isEmpty())
+ z = (ui->lineEdit_setpoint_new_z->text()).toFloat();
+ // > For x
+ if(!ui->lineEdit_setpoint_new_yaw->text().isEmpty())
+ yaw = (ui->lineEdit_setpoint_new_yaw->text()).toFloat();
+
+ // Add the increment to this
+ float x_new = x + x_inc;
+ float y_new = y + y_inc;
+ float z_new = z + z_inc;
+ float yaw_new = yaw + yaw_inc_degrees;
+
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+
+ // Put this back into the new fields
+ if (x_new < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_new_x->setText(qstr + QString::number( x_new, 'f', 3));
+ if (y_new < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_new_y->setText(qstr + QString::number( y_new, 'f', 3));
+ if (z_new < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_new_z->setText(qstr + QString::number( z_new, 'f', 3));
+
+ if (yaw_new < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_new_yaw->setText(qstr + QString::number( yaw_new, 'f', 3));
+
+ // Call the function to publish the setpoint
+ publishSetpoint(x_new,y_new,z_new,yaw_new);
+
+ }
+ else
+ {
+ #ifdef CATKIN_MAKE
+ // Inform the user that nothing can be done
+ ROS_INFO_STREAM("[STUDENT CONTROLLER GUI] Increment setpoint not possible because m_type is not recognised.");
+ #endif
+ }
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void StudentControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+ // // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("StudentControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[STUDENT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+
+ // SUBSCRIBERS
+ // > For receiving message that the setpoint was changed
+ setpointChangedSubscriber = agent_base_nodeHandle.subscribe("StudentControllerService/SetpointChanged", 1, &StudentControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // Set information back to the default
+ ui->lineEdit_setpoint_current_x->setText("xx.xx");
+ ui->lineEdit_setpoint_current_y->setText("xx.xx");
+ ui->lineEdit_setpoint_current_z->setText("xx.xx");
+ ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
+
+ }
+#endif
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
+// MM MM S G G H H E A A D D E R R
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void StudentControllerTab::fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[STUDENT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void StudentControllerTab::fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[STUDENT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool StudentControllerTab::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[STUDENT CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[STUDENT CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[STUDENT CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[STUDENT CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[STUDENT CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[STUDENT CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[STUDENT CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b0da98f058131f3786ae24f33120be55ea9254aa
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp
@@ -0,0 +1,696 @@
+// Copyright (C) 2019, 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 GUI for a Template Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "templatecontrollertab.h"
+#include "ui_templatecontrollertab.h"
+
+TemplateControllerTab::TemplateControllerTab(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::TemplateControllerTab)
+{
+ ui->setupUi(this);
+
+ // Hide the two red frames that are used to indcated
+ // when pose data is occluded
+ ui->red_frame_position_left->setVisible(false);
+ ui->red_frame_position_right->setVisible(false);
+
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER TAB GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[TEMPLATE CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
+ requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("TemplateControllerService/RequestSetpointChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("TemplateControllerService/SetpointChanged", 1, &TemplateControllerTab::setpointChangedCallback, this);
+ }
+
+ // CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
+ customButtonPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::CustomButtonWithHeader>("TemplateControllerService/CustomButtonPressed", 1);
+
+ // GET THE CURRENT SETPOINT
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("TemplateControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[TEMPLATE CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+ }
+
+#endif
+
+}
+
+TemplateControllerTab::~TemplateControllerTab()
+{
+ delete ui;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+//
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void TemplateControllerTab::publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer)
+{
+ // Initialise the message as a local variable
+ dfall_pkg::CustomButtonWithHeader msg;
+ // Fill the header of the message
+ fillCustomButtonMessageHeader( msg );
+ // Fill in the button index
+ msg.button_index = button_index;
+ // Get the line edit data, as a float if possible
+ bool isValidFloat = false;
+ float lineEdit_as_float = (lineEdit_pointer->text()).toFloat(&isValidFloat);
+ // Fill in the data
+ if (isValidFloat)
+ msg.float_data = lineEdit_as_float;
+ else
+ msg.string_data = (lineEdit_pointer->text()).toStdString();
+ // Publish the setpoint
+ this->customButtonPublisher.publish(msg);
+ // Inform the user about the change
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER TAB GUI] button " << button_index << " clicked.");
+}
+#endif
+
+
+void TemplateControllerTab::on_custom_button_1_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(1,ui->lineEdit_custom_1);
+#endif
+}
+
+void TemplateControllerTab::on_custom_button_2_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(2,ui->lineEdit_custom_2);
+#endif
+}
+
+void TemplateControllerTab::on_custom_button_3_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(3,ui->lineEdit_custom_3);
+#endif
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// PPPP OOO SSSS EEEEE DDDD A TTTTT A
+// P P O O S E D D A A T A A
+// PPPP O O SSS EEE D D A A T A A
+// P O O S E D D AAAAA T AAAAA
+// P OOO SSSS EEEEE DDDD A A T A A
+// ----------------------------------------------------------------------------------
+
+
+void TemplateControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+{
+ if (!occluded)
+ {
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+ // UPDATE THE MEASUREMENT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_x->setText(qstr + QString::number( x, 'f', 3));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_y->setText(qstr + QString::number( y, 'f', 3));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', 3));
+
+ if (roll < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_roll->setText(qstr + QString::number( roll * RAD2DEG, 'f', 1));
+ if (pitch < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_pitch->setText(qstr + QString::number( pitch * RAD2DEG, 'f', 1));
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+
+ // Ensure the red frames are not visible
+ if ( ui->red_frame_position_left->isVisible() )
+ ui->red_frame_position_left->setVisible(false);
+ if ( ui->red_frame_position_right->isVisible() )
+ ui->red_frame_position_right->setVisible(false);
+ }
+ else
+ {
+ // Make visible the red frames to indicate occluded
+ if ( !(ui->red_frame_position_left->isVisible()) )
+ ui->red_frame_position_left->setVisible(true);
+ if ( !(ui->red_frame_position_right->isVisible()) )
+ ui->red_frame_position_right->setVisible(true);
+ }
+}
+
+
+void TemplateControllerTab::poseDataUnavailableSlot()
+{
+ ui->lineEdit_measured_x->setText("xx.xx");
+ ui->lineEdit_measured_y->setText("xx.xx");
+ ui->lineEdit_measured_z->setText("xx.xx");
+
+ ui->lineEdit_measured_roll->setText("xx.xx");
+ ui->lineEdit_measured_pitch->setText("xx.xx");
+ ui->lineEdit_measured_yaw->setText("xx.xx");
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+//
+// CCCC H H A N N GGGG EEEEE DDDD
+// C H H A A NN N G E D D
+// C HHHHH A A N N N G EEE D D
+// C H H AAAAA N NN G G E D D
+// CCCC H H A A N N GGGG EEEEE DDDD
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void TemplateControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
+{
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+
+ // EXTRACT THE SETPOINT
+ float x = newSetpoint.x;
+ float y = newSetpoint.y;
+ float z = newSetpoint.z;
+ float yaw = newSetpoint.yaw;
+
+ // UPDATE THE SETPOINT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_x->setText(qstr + QString::number( x, 'f', 3));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_y->setText(qstr + QString::number( y, 'f', 3));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_z->setText(qstr + QString::number( z, 'f', 3));
+
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+}
+#endif
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE QQQ U U EEEEE SSSS TTTTT N N EEEEE W W
+// R R E Q Q U U E S T NN N E W W
+// RRRR EEE Q Q U U EEE SSS T N N N EEE W W
+// R R E Q Q U U E S T N NN E W W W
+// R R EEEEE QQ Q UUU EEEEE SSSS T N N EEEEE W W
+//
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+// ----------------------------------------------------------------------------------
+
+
+void TemplateControllerTab::publishSetpoint(float x, float y, float z, float yaw_degrees)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw_degrees * DEG2RAD;
+
+ // Publish the setpoint
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER GUI] Published request for setpoint change to: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[TEMPLATE CONTROLLER GUI] would publish request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]";
+#endif
+}
+
+
+
+void TemplateControllerTab::on_lineEdit_setpoint_new_x_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void TemplateControllerTab::on_lineEdit_setpoint_new_y_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void TemplateControllerTab::on_lineEdit_setpoint_new_z_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void TemplateControllerTab::on_lineEdit_setpoint_new_yaw_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void TemplateControllerTab::on_set_setpoint_button_clicked()
+{
+
+ // Initialise local variable for each of (x,y,z,yaw)
+ float x = 0.0f, y = 0.0f, z = 0.0f, yaw = 0.0f;
+
+ // Take the new value if available, otherwise use the old value
+ // > For x
+ if(!ui->lineEdit_setpoint_new_x->text().isEmpty())
+ x = (ui->lineEdit_setpoint_new_x->text()).toFloat();
+ else
+ x = (ui->lineEdit_setpoint_current_x->text()).toFloat();
+ // > For y
+ if(!ui->lineEdit_setpoint_new_y->text().isEmpty())
+ y = (ui->lineEdit_setpoint_new_y->text()).toFloat();
+ else
+ y = (ui->lineEdit_setpoint_current_y->text()).toFloat();
+ // > For z
+ if(!ui->lineEdit_setpoint_new_z->text().isEmpty())
+ z = (ui->lineEdit_setpoint_new_z->text()).toFloat();
+ else
+ z = (ui->lineEdit_setpoint_current_z->text()).toFloat();
+ // > For yaw
+ if(!ui->lineEdit_setpoint_new_yaw->text().isEmpty())
+ yaw = (ui->lineEdit_setpoint_new_yaw->text()).toFloat();
+ else
+ yaw = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+ // Call the function to publish the setpoint
+ publishSetpoint(x,y,z,yaw);
+}
+
+void TemplateControllerTab::on_default_setpoint_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Publish this as a blank setpoint with the
+ // "buttonID" field set appropriately
+
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.buttonID = REQUEST_DEFAULT_SETPOINT_BUTTON_ID;
+
+ // Publish the default setpoint button press
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER GUI] Published request for setpoint change to the default");
+#endif
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void TemplateControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+ // // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("TemplateControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[TEMPLATE CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+
+ // SUBSCRIBERS
+ // > For receiving message that the setpoint was changed
+ setpointChangedSubscriber = agent_base_nodeHandle.subscribe("TemplateControllerService/SetpointChanged", 1, &TemplateControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // Set information back to the default
+ ui->lineEdit_setpoint_current_x->setText("xx.xx");
+ ui->lineEdit_setpoint_current_y->setText("xx.xx");
+ ui->lineEdit_setpoint_current_z->setText("xx.xx");
+ ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
+
+ }
+#endif
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
+// MM MM S G G H H E A A D D E R R
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void TemplateControllerTab::fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[TEMPLATE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void TemplateControllerTab::fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[TEMPLATE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool TemplateControllerTab::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[TEMPLATE CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[TEMPLATE CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[TEMPLATE CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[TEMPLATE CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[TEMPLATE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b29975009c2617d65959f6a80f34e63232d5506
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
@@ -0,0 +1,428 @@
+// Copyright (C) 2019, 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 title displayed at the top of the Flying Agent GUI
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "topbanner.h"
+#include "ui_topbanner.h"
+
+#ifdef CATKIN_MAKE
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+
+TopBanner::TopBanner(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::TopBanner)
+{
+ ui->setupUi(this);
+ (":/images/rf_connected.png");
+ QPixmap pixmap(":/images/emergency_stop.png");
+ QIcon ButtonIcon(pixmap);
+ ui->emergency_stop_button->setText("");
+ ui->emergency_stop_button->setIcon(ButtonIcon);
+ ui->emergency_stop_button->setIconSize(QSize(50,50));
+ //ui->emergency_stop_button->setIconSize(pixmap.rect().size());
+
+
+ m_object_name_for_emitting_pose_data = "";
+
+
+#ifdef CATKIN_MAKE
+ // Get the namespace of this node
+ std::string base_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[TOP BANNER GUI] ros::this_node::getNamespace() = " << base_namespace);
+
+
+ // Get the type and ID of this parameter service
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the TYPE and ID are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[TOP BANNER GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+#else
+ // Default as a coordinator when compiling with QtCreator
+ m_type = TYPE_COORDINATOR;
+ m_ID = 1;
+#endif
+
+
+
+#ifdef CATKIN_MAKE
+ // CREATE A NODE HANDLE TO THE BASE NAMESPACE
+ //ros::NodeHandle base_nodeHandle(base_namespace);
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle dfall_root_nodeHandle("/dfall");
+
+ // > Publisher for the emergency stop button
+ emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("emergencyStop", 1);
+
+ // > For changes in the database that defines {agentID,cfID,flying zone} links
+ databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);;
+ centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<dfall_pkg::CMQuery>("CentralManagerService/Query", false);
+#endif
+
+
+ // FURTHER INITILIASATIONS NEED TO OCCUR AFTER THE ROS RELATED
+ // INITIALISATIONS ARE COMPLETE
+ if (m_type == TYPE_AGENT)
+ {
+ // Hide the "emergency stop"
+ ui->emergency_stop_button->hide();
+
+ // Load the context for this agent
+ loadCrazyflieContext(m_ID,1000);
+ }
+ else if (m_type == TYPE_COORDINATOR)
+ {
+ // Set the label appropriate for a cooridnator
+ QString qstr_title = "Flying Agent GUI: for COORDINATOR ID ";
+ qstr_title.append( QString::number(m_ID) );
+ ui->top_banner_label->setText(qstr_title);
+
+ // show the "emergency stop"
+ ui->emergency_stop_button->show();
+ }
+ else
+ {
+ // Set the label to inform the user of the error
+ QString qstr_title = "Flying Agent GUI: for UNKNOWN NODE TYPE";
+ ui->top_banner_label->setText(qstr_title);
+
+ // Hide the "emergency stop"
+ ui->emergency_stop_button->hide();
+ }
+
+}
+
+TopBanner::~TopBanner()
+{
+ delete ui;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC OOO N N TTTTT EEEEE X X TTTTT
+// C O O NN N T E X X T
+// C O O N N N T EEE X T
+// C O O N NN T E X X T
+// CCCC OOO N N T EEEEE X X T
+// ----------------------------------------------------------------------------------
+
+
+
+// RESPONDING TO CHANGES IN THE DATABASE
+#ifdef CATKIN_MAKE
+// > For the notification that the database was changes, received on the "DatabaseChangedSubscriber"
+void TopBanner::databaseChangedCallback(const std_msgs::Int32& msg)
+{
+ //ROS_INFO_STEAM("[TOP BANNER GUI] Database Changed Callback called for agentID = " << m_agentID);
+ loadCrazyflieContext(m_ID,0);
+}
+#endif
+
+
+
+void TopBanner::emitObjectNameForDisplayingPoseDataValueChanged()
+{
+ emit objectNameForDisplayingPoseDataValueChanged( m_object_name_for_emitting_pose_data );
+#ifdef CATKIN_MAKE
+ ROS_INFO_STREAM("[TOP BANNER GUI] Object name \"" << m_object_name_for_emitting_pose_data.toStdString() << "\" emitted for the controller tabs.");
+#endif
+}
+
+
+// > For loading the "context" for this agent, i.e., the {agentID,cfID,flying zone} tuple
+void TopBanner::loadCrazyflieContext(int ID_to_request_from_database , int emit_after_milliseconds)
+{
+
+ QString qstr_crazyflie_name = "";
+
+#ifdef CATKIN_MAKE
+ dfall_pkg::CMQuery contextCall;
+ contextCall.request.studentID = ID_to_request_from_database;
+ //ROS_INFO_STREAM("StudentID:" << m_agentID);
+
+ centralManagerDatabaseService.waitForExistence(ros::Duration(2));
+
+ if(centralManagerDatabaseService.call(contextCall))
+ {
+ my_context = contextCall.response.crazyflieContext;
+ ROS_INFO_STREAM("[TOP BANNER GUI] CrazyflieContext:\n" << my_context);
+
+ qstr_crazyflie_name.append(QString::fromStdString(my_context.crazyflieName));
+
+ m_object_name_for_emitting_pose_data = QString::fromStdString(my_context.crazyflieName);
+
+ // Emit the crazyflie name
+ // > This signal is connected to the "controller tabs" widget
+ // and is used for filtering with motion capture data to
+ // display in the tabs for each controller
+ if (emit_after_milliseconds == 0)
+ {
+ emit objectNameForDisplayingPoseDataValueChanged( QString::fromStdString(my_context.crazyflieName) );
+ ROS_INFO_STREAM("[TOP BANNER GUI] Object name \"" << my_context.crazyflieName << "\" emitted for the controller tabs.");
+ }
+ else
+ {
+ QTimer::singleShot(emit_after_milliseconds, this, SLOT( emitObjectNameForDisplayingPoseDataValueChanged() ) );
+ }
+ }
+ else
+ {
+ ROS_ERROR_STREAM("[TOP BANNER GUI] Failed to load context for agentID = " << m_ID);
+
+ // Set the Crazyflie Name String to be a question mark
+ qstr_crazyflie_name.append("?");
+
+ m_object_name_for_emitting_pose_data = "";
+
+ if (emit_after_milliseconds == 0)
+ {
+ QString qstr = "";
+ emit objectNameForDisplayingPoseDataValueChanged( qstr );
+ ROS_INFO_STREAM("[TOP BANNER GUI] emitted for the controller tabs that no object name is available.");
+ }
+ else
+ {
+ QTimer::singleShot(emit_after_milliseconds, this, SLOT( emitObjectNameForDisplayingPoseDataValueChanged() ) );
+ }
+ }
+ // This updating of the radio only needs to be done by the actual agent's node
+ //ros::NodeHandle nh("CrazyRadio");
+ //nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
+#else
+ // Set the Crazyflie Name String to be a question mark
+ qstr_crazyflie_name.append("?");
+#endif
+
+ // Construct and set the string for the checkbox label
+ QString qstr_title = "Flying Agent GUI: for AGENT ID ";
+ qstr_title.append( QString::number(m_ID) );
+ qstr_title.append(", connected to ");
+ qstr_title.append(qstr_crazyflie_name);
+ ui->top_banner_label->setText(qstr_title);
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+
+void TopBanner::on_emergency_stop_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ msg.shouldCheckForAgentID = false;
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ this->emergencyStopPublisher.publish(msg);
+ ROS_INFO("[TOP BANNER GUI] EMERGENCY STOP button clicked");
+#endif
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void TopBanner::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+ // If there is only one agent to coordinate,
+ // then load the context for that agent
+ if (agentIDs.length() == 1)
+ {
+ loadCrazyflieContext(agentIDs[0],0);
+ }
+ else
+ {
+ // Set the label appropriate for a cooridnator
+ QString qstr_title = "Flying Agent GUI: for COORDINATOR ID ";
+ qstr_title.append( QString::number(m_ID) );
+ ui->top_banner_label->setText(qstr_title);
+
+ // Update the class variable for the name
+ m_object_name_for_emitting_pose_data = "";
+ // Emit the empty name as a signal
+ QString qstr = "";
+ emit objectNameForDisplayingPoseDataValueChanged( qstr );
+ }
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool TopBanner::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[TOP BANNER GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[TOP BANNER GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_ID"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[TOP BANNER GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[TOP BANNER GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_ID"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[TOP BANNER GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[TOP BANNER GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[TOP BANNER GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad36c34dbcdae0489361f3ebc544b6f57cf085ec
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
@@ -0,0 +1,708 @@
+// Copyright (C) 2019, 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 GUI for the Tuning Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "tuningcontrollertab.h"
+#include "ui_tuningcontrollertab.h"
+
+TuningControllerTab::TuningControllerTab(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::TuningControllerTab)
+{
+ ui->setupUi(this);
+
+ // Set the gain field
+ int starting_slider_value = ui->slider_gain_P->value();
+ float slider_ratio = float(starting_slider_value) / 1000.0;
+
+ m_gain_P = P_GAIN_MIN_GUI + slider_ratio * (P_GAIN_MAX_GUI-P_GAIN_MIN_GUI);
+ m_gain_D = m_gain_P * P_TO_D_GAIN_RATIO_GUI;
+
+ ui->lineEdit_gain_P->setText( QString::number( m_gain_P, 'f', DECIMAL_PLACES_GAIN) );
+ ui->lineEdit_gain_D->setText( QString::number( m_gain_D, 'f', DECIMAL_PLACES_GAIN) );
+
+ ui->lineEdit_setpoint->setText( QString::number( m_current_setpoint, 'f', DECIMAL_PLACES_SETPOINT) );
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[TUNING CONTROLLER TAB GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[TUNING CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
+ requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("TuningControllerService/RequestSetpointChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("TuningControllerService/SetpointChanged", 1, &TuningControllerTab::setpointChangedCallback, this);
+ }
+
+
+ // CREATE THE NEW GAIN PUBLISHER
+ requestNewGainChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::FloatWithHeader>("TuningControllerService/RequestGainChange", 1);
+
+ // CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
+ //customButtonPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::CustomButtonWithHeader>("TuningControllerService/CustomButtonPressed", 1);
+
+ // GET THE CURRENT SETPOINT
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+// // > Request the current setpoint
+// ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("TuningControllerService/GetCurrentSetpoint", false);
+// dfall_pkg::GetSetpointService getSetpointCall;
+// getSetpointCall.request.data = 0;
+// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+// if(getCurrentSetpointServiceClient.call(getSetpointCall))
+// {
+// setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+// }
+// else
+// {
+// // Inform the user
+// ROS_INFO("[TUNING CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+// }
+ }
+
+#endif
+}
+
+TuningControllerTab::~TuningControllerTab()
+{
+ delete ui;
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// PPPP OOO SSSS EEEEE DDDD A TTTTT A
+// P P O O S E D D A A T A A
+// PPPP O O SSS EEE D D A A T A A
+// P O O S E D D AAAAA T AAAAA
+// P OOO SSSS EEEEE DDDD A A T A A
+// ----------------------------------------------------------------------------------
+
+
+void TuningControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+{
+ if (!occluded)
+ {
+ m_gain_setpoint_mutex.lock();
+
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+
+ // PUT IN THE POSITION ERROR
+ float error_x = x - m_current_setpoint;
+ ui->lineEdit_error_P->setText( QString::number( error_x, 'f', DECIMAL_PLACES_POSITION_MEASURED) );
+
+ // COMPUTE AND PUT IN THE VELOCITY
+ float velocity_x = (x - m_x_previous) * MEASUREMENT_FRQUENCY;
+ m_x_previous = x;
+ ui->lineEdit_error_D->setText( QString::number( velocity_x, 'f', DECIMAL_PLACES_VELOCITY) );
+
+ // COMPUTE AND PUT IN THE ANGLE
+ float angle = (m_gain_P * error_x + m_gain_D * velocity_x) * RAD2DEG;
+ ui->lineEdit_angle->setText( QString::number( angle, 'f', DECIMAL_PLACES_ANGLE_DEGREES) );
+
+ m_gain_setpoint_mutex.unlock();
+
+// // UPDATE THE MEASUREMENT COLUMN
+// if (x < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_measured_x->setText(qstr + QString::number( x, 'f', 3));
+// if (y < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_measured_y->setText(qstr + QString::number( y, 'f', 3));
+// if (z < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', 3));
+
+// if (roll < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_measured_roll->setText(qstr + QString::number( roll * RAD2DEG, 'f', 1));
+// if (pitch < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_measured_pitch->setText(qstr + QString::number( pitch * RAD2DEG, 'f', 1));
+// if (yaw < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+
+// // GET THE CURRENT SETPOINT
+// float error_x = x - (ui->lineEdit_setpoint_current_x->text() ).toFloat();
+// float error_y = y - (ui->lineEdit_setpoint_current_y->text() ).toFloat();
+// float error_z = z - (ui->lineEdit_setpoint_current_z->text() ).toFloat();
+// float error_yaw = yaw - (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+// // UPDATE THE ERROR COLUMN
+// if (error_x < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_error_x->setText(qstr + QString::number( error_x, 'f', 3));
+// if (error_y < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_error_y->setText(qstr + QString::number( error_y, 'f', 3));
+// if (error_z < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', 3));
+
+// if (error_yaw < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw * RAD2DEG, 'f', 1));
+
+// // Ensure the red frames are not visible
+// if ( ui->red_frame_position_left->isVisible() )
+// ui->red_frame_position_left->setVisible(false);
+// if ( ui->red_frame_position_right->isVisible() )
+// ui->red_frame_position_right->setVisible(false);
+ }
+ else
+ {
+// // Make visible the red frames to indicate occluded
+// if ( !(ui->red_frame_position_left->isVisible()) )
+// ui->red_frame_position_left->setVisible(true);
+// if ( !(ui->red_frame_position_right->isVisible()) )
+// ui->red_frame_position_right->setVisible(true);
+ }
+}
+
+
+void TuningControllerTab::poseDataUnavailableSlot()
+{
+ ui->lineEdit_angle->setText("xx.xx");
+ ui->lineEdit_error_P->setText("xx.xx");
+ ui->lineEdit_error_D->setText("xx.xx");
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+//
+// CCCC H H A N N GGGG EEEEE DDDD
+// C H H A A NN N G E D D
+// C HHHHH A A N N N G EEE D D
+// C H H AAAAA N NN G G E D D
+// CCCC H H A A N N GGGG EEEEE DDDD
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void TuningControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
+{
+// // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+// QString qstr = "";
+
+// // EXTRACT THE SETPOINT
+// float x = newSetpoint.x;
+// float y = newSetpoint.y;
+// float z = newSetpoint.z;
+// float yaw = newSetpoint.yaw;
+
+// // UPDATE THE SETPOINT COLUMN
+// if (x < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_setpoint_current_x->setText(qstr + QString::number( x, 'f', 3));
+// if (y < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_setpoint_current_y->setText(qstr + QString::number( y, 'f', 3));
+// if (z < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_setpoint_current_z->setText(qstr + QString::number( z, 'f', 3));
+
+// if (yaw < 0.0f) qstr = ""; else qstr = "+";
+// ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 3));
+}
+#endif
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE QQQ U U EEEEE SSSS TTTTT N N EEEEE W W
+// R R E Q Q U U E S T NN N E W W
+// RRRR EEE Q Q U U EEE SSS T N N N EEE W W
+// R R E Q Q U U E S T N NN E W W W
+// R R EEEEE QQ Q UUU EEEEE SSSS T N N EEEEE W W
+//
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+// ----------------------------------------------------------------------------------
+
+
+void TuningControllerTab::publishSetpoint(float x, float y, float z, float yaw)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw * DEG2RAD;
+
+ // Publish the setpoint
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[TUNING CONTROLLER GUI] Published request for setpoint change to: [" << x << ", "<< y << ", "<< z << ", "<< yaw << "]");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[TUNING CONTROLLER GUI] would publish request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw << "]";
+#endif
+}
+
+
+
+
+
+void TuningControllerTab::on_button_setpoint_toggle_clicked()
+{
+
+ m_gain_setpoint_mutex.lock();
+
+ float setpoint_copy = m_current_setpoint;
+ float new_setpoint = 0.0;
+
+ if (setpoint_copy < SETPOINT_X_MINUS)
+ new_setpoint = SETPOINT_X_MINUS;
+ else if (setpoint_copy < 0.0)
+ new_setpoint = SETPOINT_X_PLUS;
+ else if (setpoint_copy > SETPOINT_X_PLUS)
+ new_setpoint = SETPOINT_X_PLUS;
+ else
+ new_setpoint = SETPOINT_X_MINUS;
+
+ ui->lineEdit_setpoint->setText( QString::number( new_setpoint, 'f', DECIMAL_PLACES_SETPOINT) );
+ float lineEdit_as_float_rounded = (ui->lineEdit_setpoint->text()).toFloat();
+ m_current_setpoint = lineEdit_as_float_rounded;
+ publishSetpoint( m_current_setpoint, SETPOINT_Y, SETPOINT_Z, SETPOINT_YAW_DEGREES);
+
+ m_gain_setpoint_mutex.unlock();
+
+}
+
+void TuningControllerTab::on_lineEdit_setpoint_editingFinished()
+{
+ m_gain_setpoint_mutex.lock();
+
+
+ // Get the line edit data, as a float if possible
+ bool isValidFloat = false;
+ float lineEdit_as_float = (ui->lineEdit_setpoint->text()).toFloat(&isValidFloat);
+
+ // Fill in the data
+ if (isValidFloat)
+ {
+ ui->lineEdit_setpoint->setText( QString::number( lineEdit_as_float, 'f', DECIMAL_PLACES_SETPOINT) );
+ float lineEdit_as_float_rounded = (ui->lineEdit_setpoint->text()).toFloat();
+ m_current_setpoint = lineEdit_as_float_rounded;
+ publishSetpoint( m_current_setpoint, SETPOINT_Y, SETPOINT_Z, SETPOINT_YAW_DEGREES);
+ }
+ else
+ {
+ ui->lineEdit_setpoint->setText( QString::number( m_current_setpoint, 'f', DECIMAL_PLACES_SETPOINT) );
+ }
+ m_gain_setpoint_mutex.unlock();
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+void TuningControllerTab::publishGain(float new_gain)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::FloatWithHeader msg;
+
+ // Fill the header of the message
+ fillFloatMessageHeader( msg );
+
+ // Fill in the gain value
+ msg.data = new_gain;
+
+ // Publish the setpoint
+ this->requestNewGainChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[TUNING CONTROLLER GUI] Published request for gain change to: " << new_gain );
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[TUNING CONTROLLER GUI] would publish gain value: " << new_gain;
+#endif
+}
+
+
+void TuningControllerTab::on_slider_gain_P_valueChanged(int value)
+{
+
+ m_gain_setpoint_mutex.lock();
+
+ float slider_ratio = float(value) / 1000.0;
+
+ float new_gain_P = P_GAIN_MIN_GUI + slider_ratio * (P_GAIN_MAX_GUI-P_GAIN_MIN_GUI);
+ float new_gain_D = new_gain_P * P_TO_D_GAIN_RATIO_GUI;
+
+ ui->lineEdit_gain_P->setText( QString::number( new_gain_P, 'f', DECIMAL_PLACES_GAIN) );
+ ui->lineEdit_gain_D->setText( QString::number( new_gain_D, 'f', DECIMAL_PLACES_GAIN) );
+
+ float gain_P_rounded = (ui->lineEdit_gain_P->text()).toFloat();
+ float gain_D_rounded = (ui->lineEdit_gain_D->text()).toFloat();
+
+ m_gain_P = gain_P_rounded;
+ m_gain_D = gain_D_rounded;
+
+ publishGain(m_gain_P);
+
+ m_gain_setpoint_mutex.unlock();
+
+}
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void TuningControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+// // // > Create the appropriate node handle
+// QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+// ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+// // // > Request the current setpoint
+// ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("TuningControllerService/GetCurrentSetpoint", false);
+// dfall_pkg::GetSetpointService getSetpointCall;
+// getSetpointCall.request.data = 0;
+// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+// if(getCurrentSetpointServiceClient.call(getSetpointCall))
+// {
+// setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+// }
+// else
+// {
+// // Inform the user
+// ROS_INFO("[TUNING CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+// }
+
+// // SUBSCRIBERS
+// // > For receiving message that the setpoint was changed
+// setpointChangedSubscriber = agent_base_nodeHandle.subscribe("TuningControllerService/SetpointChanged", 1, &TuningControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // Set information back to the default
+ ui->lineEdit_setpoint->setText("xx.xx");
+ //ui->lineEdit_setpoint_current_y->setText("xx.xx");
+ //ui->lineEdit_setpoint_current_z->setText("xx.xx");
+ //ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
+
+ }
+#endif
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
+// MM MM S G G H H E A A D D E R R
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void TuningControllerTab::fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[TUNING CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void TuningControllerTab::fillFloatMessageHeader( dfall_pkg::FloatWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[TUNING CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool TuningControllerTab::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[TUNING CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[TUNING CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[TUNING CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[TUNING CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[TUNING CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[TUNING CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[TUNING CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/forms/mainguiwindow.ui
similarity index 73%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.ui
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/forms/mainguiwindow.ui
index 737f460b0ae385e2d25c23aabc6eb990df265ec6..2d50c2a9dd741d735d515d406cda65846f8db683 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/forms/mainguiwindow.ui
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1176</width>
+ <width>1228</width>
<height>1559</height>
</rect>
</property>
@@ -273,7 +273,7 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
- <widget class="QPushButton" name="all_motors_off_button">
+ <widget class="QPushButton" name="emergency_stop_button">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -300,7 +300,7 @@
</font>
</property>
<property name="text">
- <string>All motors OFF</string>
+ <string>Emergency Stop</string>
</property>
</widget>
</item>
@@ -870,329 +870,56 @@
</widget>
<widget class="QWidget" name="tab">
<attribute name="title">
- <string>Command all</string>
+ <string>Mocap</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_2">
- <item row="10" column="1">
- <widget class="QPushButton" name="all_load_custom_controller_yaml_coordinator_button">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Load Custom YAML</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0" colspan="2">
- <widget class="QLabel" name="all_yaml_label">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>20</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>40</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>LOAD YAML PARAMETERS</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignBottom|Qt::AlignHCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QPushButton" name="all_take_off_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Take off</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QPushButton" name="all_connect_button">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Reconnect</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0" colspan="2">
- <widget class="QLabel" name="all_flying_state_label">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>20</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>40</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>FLYING STATE</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignBottom|Qt::AlignHCenter</set>
- </property>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QPushButton" name="all_enable_safe_controller_button">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Enable Safe</string>
- </property>
- </widget>
- </item>
- <item row="8" column="1">
- <widget class="QPushButton" name="all_load_custom_controller_yaml_own_agent_button">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Load Custom YAML</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QPushButton" name="all_disconnect_button">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Disconnect</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QPushButton" name="all_enable_custom_controller_button">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Enable Custom</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QPushButton" name="all_land_button">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Land</string>
- </property>
- </widget>
- </item>
- <item row="8" column="0">
- <widget class="QPushButton" name="all_load_safe_controller_yaml_own_agent_button">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Load Safe YAML</string>
- </property>
- </widget>
- </item>
- <item row="7" column="0" colspan="2">
- <widget class="QLabel" name="all_yaml_agent_label">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>30</height>
- </size>
- </property>
- <property name="text">
- <string>> From agenet's local file</string>
- </property>
- </widget>
- </item>
- <item row="10" column="0">
- <widget class="QPushButton" name="all_load_safe_controller_yaml_coordinator_button">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Load Safe YAML</string>
- </property>
- </widget>
- </item>
- <item row="9" column="0" colspan="2">
- <widget class="QLabel" name="all_yaml_coordinator_label">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>30</height>
- </size>
- </property>
- <property name="text">
- <string>> From coordinator's file</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0" colspan="2">
- <widget class="QLabel" name="all_radio_label">
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>20</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>40</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="lineWidth">
- <number>1</number>
- </property>
- <property name="text">
- <string>CRAZYRADIO</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignBottom|Qt::AlignHCenter</set>
- </property>
- </widget>
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="0" column="0">
+ <widget class="QLabel" name="all_radio_label">
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>20</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="lineWidth">
+ <number>1</number>
+ </property>
+ <property name="text">
+ <string>Work in progress.</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignBottom|Qt::AlignHCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" 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>
+ </layout>
</item>
</layout>
</widget>
@@ -1245,8 +972,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1176</width>
- <height>25</height>
+ <width>1228</width>
+ <height>40</height>
</rect>
</property>
</widget>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/center_rect.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/center_rect.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/center_rect.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/center_rect.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone.png b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone.png
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone.png
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone.png
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_01.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_01.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_01.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_01.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_02.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_02.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_02.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_02.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_03.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_03.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_03.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_03.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_04.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_04.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_04.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_04.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_05.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_05.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_05.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_05.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_06.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_06.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_06.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_06.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_07.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_07.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_07.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_07.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_08.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_08.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_08.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_08.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_09.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_09.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_09.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_09.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_unk.svg b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_unk.svg
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/images/drone_fixed_unk.svg
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/images/drone_fixed_unk.svg
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/CFLinker.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/CFLinker.h
similarity index 98%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/CFLinker.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/CFLinker.h
index a0a9f56858f281d4d17a3090cda66b4545f9802b..b0eb2064738bb4f6b3ada43f41e9ddf8a7769c9d 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/CFLinker.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/CFLinker.h
@@ -38,7 +38,7 @@
#include "crazyFlyZone.h"
#include "ui_mainguiwindow.h"
-#include "rosNodeThread_for_managerGUI.h"
+#include "rosNodeThread_for_systemConfigGUI.h"
#include <QObject>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/centerMarker.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/centerMarker.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/centerMarker.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/centerMarker.h
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/channelLUT.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/channelLUT.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/channelLUT.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/channelLUT.h
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/constants_for_qt_compile.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/constants_for_qt_compile.h
new file mode 100644
index 0000000000000000000000000000000000000000..891b10ef3511568f97a7026dd6800461925dc4c8
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/constants_for_qt_compile.h
@@ -0,0 +1,209 @@
+// 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:
+// Constants that are used across multiple files
+//
+// ----------------------------------------------------------------------------------
+
+
+// ----------------------------------------------------------------------------------
+// U U
+// U U
+// U U
+// U U
+// UUU
+// ----------------------------------------------------------------------------------
+
+
+// Conversions between degrees and radians
+#define RAD2DEG 180.0/PI
+#define DEG2RAD PI/180.0
+
+// PI
+#define PI 3.141592653589
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// The types, i.e., agent or coordinator
+#define TYPE_INVALID -1
+#define TYPE_COORDINATOR 1
+#define TYPE_AGENT 2
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board the Crazyflie 2.0.
+// The following is a short description about each mode:
+//
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+//
+// CF_COMMAND_TYPE_RATE
+// 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.
+//
+// CF_COMMAND_TYPE_ANGLE
+// 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_MOTORS 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+// Types of controllers being used:
+#define DEFAULT_CONTROLLER 1
+#define DEMO_CONTROLLER 2
+#define STUDENT_CONTROLLER 3
+#define MPC_CONTROLLER 4
+#define REMOTE_CONTROLLER 5
+#define TUNING_CONTROLLER 6
+#define PICKER_CONTROLLER 7
+
+// The constants that "command" changes in the
+// operation state of this agent
+#define CMD_USE_DEFAULT_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_USE_PICKER_CONTROLLER 7
+
+
+#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
+#define STATE_UNAVAILABLE 5
+
+
+// Commands for CrazyRadio
+#define CMD_RECONNECT 0
+#define CMD_DISCONNECT 1
+
+
+// CrazyRadio states:
+#define CRAZY_RADIO_STATE_CONNECTED 0
+#define CRAZY_RADIO_STATE_CONNECTING 1
+#define CRAZY_RADIO_STATE_DISCONNECTED 2
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
+// ----------------------------------------------------------------------------------
+
+// Battery levels
+#define BATTERY_LEVEL_000 0
+#define BATTERY_LEVEL_010 1
+#define BATTERY_LEVEL_020 2
+#define BATTERY_LEVEL_030 3
+#define BATTERY_LEVEL_040 4
+#define BATTERY_LEVEL_050 5
+#define BATTERY_LEVEL_060 6
+#define BATTERY_LEVEL_070 7
+#define BATTERY_LEVEL_080 8
+#define BATTERY_LEVEL_090 9
+#define BATTERY_LEVEL_100 10
+#define BATTERY_LEVEL_UNAVAILABLE -1
+
+// Battery states
+#define BATTERY_STATE_NORMAL 0
+#define BATTERY_STATE_LOW 1
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// Y Y A M M L
+// Y Y A A MM MM L
+// Y A A M M M L
+// Y AAAAA M M L
+// Y A A M M LLLLL
+// ----------------------------------------------------------------------------------
+
+// For where to load the yaml file from
+#define LOAD_YAML_FROM_AGENT 1
+#define LOAD_YAML_FROM_COORDINATOR 2
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// For standard buttons in the GUI
+#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/cornergrabber.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/cornergrabber.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/cornergrabber.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/cornergrabber.h
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFly.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFly.h
similarity index 91%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFly.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFly.h
index dc81898794c3585a14370fe8e823431f8b523127..fe7ec9b6ac53088164d4656cf6a5e8975d12d320 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFly.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFly.h
@@ -38,17 +38,28 @@
#include <QGraphicsSvgItem>
#include <QSvgRenderer>
+
#ifdef CATKIN_MAKE
-#include "d_fall_pps/CrazyflieData.h"
+// Include the Crazyflie Data Struct
+#include "dfall_pkg/CrazyflieData.h"
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+#else
+// Include the shared definitions
+#include "include/constants_for_qt_compile.h"
#endif
+
+
#ifdef CATKIN_MAKE
-using namespace d_fall_pps;
+using namespace dfall_pkg;
#endif
+
#define DRONE_HEIGHT 100 * FROM_MILIMETERS_TO_UNITS * 1.2
#define DRONE_WIDTH 100 * FROM_MILIMETERS_TO_UNITS * 1.2
+
class crazyFly : public QGraphicsSvgItem
{
public:
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFlyZone.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFlyZone.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFlyZone.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFlyZone.h
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFlyZoneTab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFlyZoneTab.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/crazyFlyZoneTab.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/crazyFlyZoneTab.h
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/globalDefinitions.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/globalDefinitions.h
similarity index 91%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/globalDefinitions.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/globalDefinitions.h
index c25a843d718c0802e87beea70de7812a9d329482..2a85b0fc3bbc6f73d4db68b331f66585067413e8 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/globalDefinitions.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/globalDefinitions.h
@@ -41,9 +41,4 @@
#define FROM_CENTIMETERS_TO_UNITS 1
#define FROM_MILIMETERS_TO_UNITS 0.1
-#define PI 3.1415926
-
-#define FROM_RADIANS_TO_DEGREES 180.0/PI
-#define FROM_DEGREES_TO_RADIANS PI/180.0
-
#endif
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
similarity index 62%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
index 96a9f12bc5654d80e9f2cfa115303467457f675e..abe9d659af917b9208e482dae0b6b5597bdfc4d4 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
@@ -41,38 +41,36 @@
#ifdef CATKIN_MAKE
-#include "rosNodeThread_for_managerGUI.h"
+#include "rosNodeThread_for_systemConfigGUI.h"
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
#include "marker.h"
#include "crazyFly.h"
#include "CFLinker.h"
-#include "d_fall_pps/CrazyflieDB.h"
-#include "d_fall_pps/CrazyflieEntry.h"
-
-#include <std_msgs/Int32.h>
+// Include the DFALL service types
+#include "dfall_pkg/CrazyflieDB.h"
+#include "dfall_pkg/CrazyflieEntry.h"
+// Include the shared definitions
+#include "nodes/Constants.h"
-// The constants that are sent to the agents in order to
-// "command" changes in their operation state
-#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
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
-// The constants that are sent to the agents in order to
-// adjust their radio connection
-#define CMD_RECONNECT 0
-#define CMD_DISCONNECT 1
-// For which controller parameters to load
-#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
-#define LOAD_YAML_CUSTOM_CONTROLLER_AGENT 2
-#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 3
-#define LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
+using namespace dfall_pkg;
-using namespace d_fall_pps;
+#else
+// Include the shared definitions
+#include "include/constants_for_qt_compile.h"
#endif
@@ -164,12 +162,14 @@ private slots:
void on_load_from_DB_button_clicked();
- #ifdef CATKIN_MAKE
+#ifdef CATKIN_MAKE
void updateNewViconData(const ptrToMessage& p_msg);
- #endif
+#endif
+
void on_checkBox_vicon_crazyflies_toggled(bool checked);
void on_scaleSpinBox_valueChanged(double arg1);
+
void on_refresh_cfs_button_clicked();
void on_refresh_student_ids_button_clicked();
@@ -181,27 +181,13 @@ private slots:
void updateComboBoxes();
void setTabIndex(int index);
+
void doTabClosed(int tab_index);
void on_comboBoxCFs_currentTextChanged(const QString &arg1);
-
- // For the buttons that "command" all of the agent nodes
- // > For the radio connection
- void on_all_connect_button_clicked();
- void on_all_disconnect_button_clicked();
- // > For changing the operation state
- void on_all_take_off_button_clicked();
- void on_all_land_button_clicked();
- void on_all_motors_off_button_clicked();
- void on_all_enable_safe_controller_button_clicked();
- void on_all_enable_custom_controller_button_clicked();
- // > For loading the parameter
- void on_all_load_safe_controller_yaml_own_agent_button_clicked();
- void on_all_load_custom_controller_yaml_own_agent_button_clicked();
- // > For sending a message with updated parameters
- void on_all_load_safe_controller_yaml_coordinator_button_clicked();
- void on_all_load_custom_controller_yaml_coordinator_button_clicked();
+ // For the emergency stop button
+ void on_emergency_stop_button_clicked();
private:
@@ -209,25 +195,19 @@ private:
Ui::MainGUIWindow *ui;
myGraphicsScene* scene;
- ros::Timer m_timer_yaml_file_for_safe_controller;
- ros::Timer m_timer_yaml_file_for_custom_controller;
-
void _init();
- void safeYamlFileTimerCallback(const ros::TimerEvent&);
- void customYamlFileTimerCallback(const ros::TimerEvent&);
-
- void customSendYamlAsMessageTimerCallback(const ros::TimerEvent&);
-
+#ifdef CATKIN_MAKE
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);
+#endif
- #ifdef CATKIN_MAKE
+#ifdef CATKIN_MAKE
rosNodeThread* _rosNodeThread;
std::vector<Marker*> markers_vector;
std::vector<crazyFly*> crazyflies_vector;
@@ -235,28 +215,9 @@ private:
std::string ros_namespace;
- ros::Publisher DBChangedPublisher;
+ //ros::Publisher DBChangedPublisher;
ros::Publisher emergencyStopPublisher;
-
- // Publsher for sending "commands" from here (the master) to all
- // of the agent nodes (where a "command" is the integer that
- // gives the directive to "take-off", "land, "motors-off", etc...)
- ros::Publisher commandAllAgentsPublisher;
-
- // Publisher for sending a request from here (the master) to all "Parameter Service" nodes
- // that it should re-load parameters from the YAML file for the controllers.
- // > This is recieved and acted on by both Coordinate and Agent type "Parameter Services",
- // > A coordinator type "Parameter Service" will subsequently request the agents to fetch
- // the parameters from itself.
- // > A agent type "Parameter Service" will subsequently request its own agent to fetch
- // the parameters from itself.
- ros::Publisher requestLoadControllerYamlPublisher;
-
- // Publisher for sending a request from here (the master) to all
- // of the agents nodes that they should (re/dis)-connect from
- // the Crazy-Radio
- ros::Publisher crazyRadioCommandAllAgentsPublisher;
- #endif
+#endif
void updateComboBoxesCFs();
@@ -264,6 +225,8 @@ private:
int getTabIndexFromName(QString name);
+
+#ifdef CATKIN_MAKE
CrazyflieDB m_data_base;
void clear_database_file();
@@ -275,6 +238,7 @@ private:
void save_database_file();
void insert_or_update_entry_database(CrazyflieEntry entry);
+#endif
};
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/marker.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/marker.h
similarity index 97%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/marker.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/marker.h
index 1a7a4ae0d4a2f4bb5035003f5c6292dff6990c34..8aa7132833c8a880e7d28f77873c6894fd2138ba 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/marker.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/marker.h
@@ -38,12 +38,12 @@
#include <QGraphicsEllipseItem>
#ifdef CATKIN_MAKE
-#include "d_fall_pps/UnlabeledMarker.h"
+#include "dfall_pkg/UnlabeledMarker.h"
#endif
#ifdef CATKIN_MAKE
-using namespace d_fall_pps;
+using namespace dfall_pkg;
#endif
#define MARKER_DIAMETER 20 * FROM_MILIMETERS_TO_UNITS
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/myGraphicsRectItem.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/myGraphicsRectItem.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/myGraphicsRectItem.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/myGraphicsRectItem.h
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/myGraphicsScene.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/myGraphicsScene.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/myGraphicsScene.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/myGraphicsScene.h
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/myGraphicsView.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/myGraphicsView.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/myGraphicsView.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/myGraphicsView.h
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/rosNodeThread_for_managerGUI.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/rosNodeThread_for_systemConfigGUI.h
similarity index 91%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/rosNodeThread_for_managerGUI.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/rosNodeThread_for_systemConfigGUI.h
index e07356ee7c505e8cce81bd51b1723a939f73f792..1a4f037cd6da979a8966426a1605ef1a639889b0 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/rosNodeThread_for_managerGUI.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/rosNodeThread_for_systemConfigGUI.h
@@ -30,8 +30,8 @@
// ----------------------------------------------------------------------------------
-#ifndef ___ROSNODETHREAD_FOR_MANAGERGUI_H___
-#define ___ROSNODETHREAD_FOR_MANAGERGUI_H___
+#ifndef ___ROSNODETHREAD_FOR_SYSTEMCONFIGGUI_H___
+#define ___ROSNODETHREAD_FOR_SYSTEMCONFIGGUI_H___
#include <QtCore>
#include <QThread>
@@ -43,11 +43,11 @@
#include <ros/ros.h>
#include <ros/network.h>
-#include "d_fall_pps/UnlabeledMarker.h"
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/ViconData.h"
+#include "dfall_pkg/UnlabeledMarker.h"
+#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/ViconData.h"
-using namespace d_fall_pps;
+using namespace dfall_pkg;
typedef ViconData::ConstPtr ptrToMessage;
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/tablePiece.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/tablePiece.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/tablePiece.h
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/tablePiece.h
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/CFLinker.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/CFLinker.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/CFLinker.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/CFLinker.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/centerMarker.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/centerMarker.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/centerMarker.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/centerMarker.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/channelLUT.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/channelLUT.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/channelLUT.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/channelLUT.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/cornergrabber.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/cornergrabber.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/cornergrabber.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/cornergrabber.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/crazyFly.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFly.cpp
similarity index 96%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/crazyFly.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFly.cpp
index d83ccd4e464d1060e58ef03ec5c410dbbccafb29..8d40b6eb34e32b25f04a85b4ad78fd5779a17b0b 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/crazyFly.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFly.cpp
@@ -88,7 +88,7 @@ void crazyFly::updateCF(const CrazyflieData* p_crazyfly_msg)
this->setPos(m_x * FROM_METERS_TO_UNITS, -m_y * FROM_METERS_TO_UNITS); // - y because of coordinates
- this->setRotation(- m_yaw * FROM_RADIANS_TO_DEGREES); //negative beacause anti-clock wise should be positive
+ this->setRotation(- m_yaw * RAD2DEG); //negative beacause anti-clock wise should be positive
}
}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/crazyFlyZone.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFlyZone.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/crazyFlyZone.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFlyZone.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/crazyFlyZoneTab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFlyZoneTab.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/crazyFlyZoneTab.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/crazyFlyZoneTab.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/main.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/main.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/main.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/main.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
similarity index 67%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
index 8cd1e74cc8c9262b120c10e80bba33f3697608f3..b8615d178ff566ddb396a81149aef86b6b34a7a0 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
@@ -47,11 +47,11 @@
#include <QShortcut>
#ifdef CATKIN_MAKE
-#include "d_fall_pps/UnlabeledMarker.h"
-#include "d_fall_pps/CMRead.h"
-#include "d_fall_pps/CrazyflieEntry.h"
-#include "d_fall_pps/CMUpdate.h"
-#include "d_fall_pps/CMCommand.h"
+#include "dfall_pkg/UnlabeledMarker.h"
+#include "dfall_pkg/CMRead.h"
+#include "dfall_pkg/CrazyflieEntry.h"
+#include "dfall_pkg/CMUpdate.h"
+#include "dfall_pkg/CMCommand.h"
#include "nodes/CentralManagerService.h"
#include <ros/ros.h>
@@ -65,7 +65,7 @@
#define N_MAX_CRAZYFLIES 20 // protection number
#ifdef CATKIN_MAKE
-using namespace d_fall_pps;
+using namespace dfall_pkg;
#endif
MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
@@ -73,7 +73,7 @@ MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
ui(new Ui::MainGUIWindow)
{
#ifdef CATKIN_MAKE
- _rosNodeThread = new rosNodeThread(argc, argv, "my_GUI");
+ _rosNodeThread = new rosNodeThread(argc, argv, "SystemConfigGUI");
#endif
ui->setupUi(this);
_init();
@@ -260,12 +260,12 @@ void MainGUIWindow::_init()
// Add keyboard shortcuts
// > for "all motors off", press the space bar
- ui->all_motors_off_button->setShortcut(tr("Space"));
+ ui->emergency_stop_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
+#ifdef CATKIN_MAKE
_rosNodeThread->init();
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
@@ -274,28 +274,13 @@ void MainGUIWindow::_init()
ros_namespace = ros::this_node::getNamespace();
- ros::NodeHandle nodeHandle("~");
- DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
- emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
-
- // Initialise the publisher for sending "commands" from here (the master)
- // to all of the agent nodes
- commandAllAgentsPublisher = nodeHandle.advertise<std_msgs::Int32>("commandAllAgents", 1);
-
- // Publisher for sending a request from here (the master) to all "Parameter Service" nodes
- // that it should re-load parameters from the YAML file for the controllers.
- // > This is recieved and acted on by both Coordinate and Agent type "Parameter Services",
- // > A coordinator type "Parameter Service" will subsequently request the agents to fetch
- // the parameters from itself.
- // > A agent type "Parameter Service" will subsequently request its own agent to fetch
- // the parameters from itself.
- requestLoadControllerYamlPublisher = nodeHandle.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
-
- // Initialise the publisher for sending a request from here (the master)
- // to all of the agents nodes that they should (re/dis)-connect from
- // the Crazy-Radio
- crazyRadioCommandAllAgentsPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommandAllAgents", 1);
- #endif
+ //ros::NodeHandle nodeHandle("~");
+ //DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+ emergencyStopPublisher = nodeHandle_dfall_root.advertise<dfall_pkg::IntWithHeader>("emergencyStop", 1);
+#endif
}
void MainGUIWindow::doTabClosed(int tab_index)
@@ -739,17 +724,17 @@ void MainGUIWindow::on_refresh_student_ids_button_clicked()
#ifdef CATKIN_MAKE
ui->list_discovered_student_ids->clear();
- // \/(\d)\/PPSClient
+ // \/(\d)\/FlyingAgentClient
ros::V_string v_str;
ros::master::getNodes(v_str);
for(int i = 0; i < v_str.size(); i++)
{
std::string s = v_str[i];
std::smatch m;
- //std::regex e ("\\/(\\d)\\/PPSClient");
- std::regex e ("\\/agent(\\d\\d\\d)\\/PPSClient");
+ //std::regex e ("\\/(\\d)\\/FlyingAgentClient");
+ std::regex e ("\\/agent(\\d\\d\\d)\\/FlyingAgentClient");
- // std::regex e("\\/PPSClien(.)");
+ // std::regex e("\\/FlyingAgentClient(.)");
// while(std::regex_search(s, m, e))
// {
@@ -842,6 +827,7 @@ void MainGUIWindow::on_unlink_button_clicked()
void MainGUIWindow::on_save_in_DB_button_clicked()
{
+#ifdef CATKIN_MAKE
// we need to update and then save?
CrazyflieDB tmp_db;
for(int i = 0; i < cf_linker->links.size(); i++)
@@ -868,7 +854,7 @@ void MainGUIWindow::on_save_in_DB_button_clicked()
tmp_entry.crazyflieContext.localArea.ymax = y_max * FROM_UNITS_TO_METERS;
tmp_entry.crazyflieContext.localArea.zmin = -0.2;
- tmp_entry.crazyflieContext.localArea.zmax = 1.2;
+ tmp_entry.crazyflieContext.localArea.zmax = 2.2;
}
}
tmp_db.crazyflieEntries.push_back(tmp_entry);
@@ -883,11 +869,14 @@ void MainGUIWindow::on_save_in_DB_button_clicked()
fill_database_file();
// Now also publish a ROS message stating that we changed the DB, so the nodes can update it
- std_msgs::Int32 msg;
- msg.data = 1;
- this->DBChangedPublisher.publish(msg);
+ //std_msgs::Int32 msg;
+ //msg.data = 1;
+ //this->DBChangedPublisher.publish(msg);
+#endif
}
+
+#ifdef CATKIN_MAKE
void MainGUIWindow::clear_database_file()
{
CrazyflieDB tmp_db;
@@ -914,7 +903,10 @@ void MainGUIWindow::clear_database_file()
ROS_INFO("Failed to read DB");
}
}
+#endif
+
+#ifdef CATKIN_MAKE
void MainGUIWindow::fill_database_file()
{
clear_database_file();
@@ -927,7 +919,10 @@ void MainGUIWindow::fill_database_file()
}
save_database_file();
}
+#endif
+
+#ifdef CATKIN_MAKE
void MainGUIWindow::save_database_file()
{
CMCommand commandCall;
@@ -941,7 +936,10 @@ void MainGUIWindow::save_database_file()
ROS_ERROR("failed to save db");
}
}
+#endif
+
+#ifdef CATKIN_MAKE
void MainGUIWindow::insert_or_update_entry_database(CrazyflieEntry entry)
{
CMUpdate updateCall;
@@ -949,7 +947,10 @@ void MainGUIWindow::insert_or_update_entry_database(CrazyflieEntry entry)
updateCall.request.crazyflieEntry = entry;
_rosNodeThread->m_update_db_client.call(updateCall);
}
+#endif
+
+#ifdef CATKIN_MAKE
int MainGUIWindow::read_database_from_file(CrazyflieDB &read_db)
{
CMRead getDBCall;
@@ -964,9 +965,12 @@ int MainGUIWindow::read_database_from_file(CrazyflieDB &read_db)
return -1;
}
}
+#endif
+
void MainGUIWindow::on_load_from_DB_button_clicked()
{
+#ifdef CATKIN_MAKE
CrazyflieDB tmp_db;
if(read_database_from_file(tmp_db) == 0)
@@ -1036,6 +1040,7 @@ void MainGUIWindow::on_load_from_DB_button_clicked()
{
ROS_ERROR("Failed to read DB");
}
+#endif
}
void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
@@ -1050,7 +1055,9 @@ void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
}
else
{
+ #ifdef CATKIN_MAKE
ROS_INFO("name not found in LUT");
+ #endif
}
}
@@ -1059,312 +1066,27 @@ void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
// ----------------------------------------------------------------------------------
-// CCCC OOO M M M M A N N DDDD A L L
-// C O O MM MM MM MM A A NN N D D A A L L
-// C O O M M M M M M A A N N N D D A A L L
-// C O O M M M M AAAAA N NN D D AAAAA L L
-// CCCC OOO M M M M A A N N DDDD A A LLLLL LLLLL
+// EEEEE M M EEEEE RRRR GGGG EEEEE N N CCCC Y Y
+// E MM MM E R R G E NN N C Y Y
+// EEE M M M EEE RRRR G GG EEE N N N C Y
+// E M M E R R G G E N NN C Y
+// EEEEE M M EEEEE R R GGG EEEEE N N CCCC Y
//
-// BBBB U U TTTTT TTTTT OOO N N SSSS
-// B B U U T T O O NN N S
-// BBBB U U T T O O N N N SSS
-// B B U U T T O O N NN S
-// BBBB UUU T T OOO N N SSSS
+// SSSS TTTTT OOO PPPP
+// S T O O P P
+// SSS T O O PPPP
+// S T O O P
+// SSSS T OOO P
// ----------------------------------------------------------------------------------
-// For the buttons that commands all of the agent nodes to:
-// > (RE)CONNECT THE RADIO
-void MainGUIWindow::on_all_connect_button_clicked()
+// > EMERGENCY STOP
+void MainGUIWindow::on_emergency_stop_button_clicked()
{
- std_msgs::Int32 msg;
- msg.data = CMD_RECONNECT;
- crazyRadioCommandAllAgentsPublisher.publish(msg);
-}
-// > DISCONNECT THE RADIO
-void MainGUIWindow::on_all_disconnect_button_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_DISCONNECT;
- crazyRadioCommandAllAgentsPublisher.publish(msg);
-}
-// > TAKE-OFF
-void MainGUIWindow::on_all_take_off_button_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_CRAZYFLY_TAKE_OFF;
- commandAllAgentsPublisher.publish(msg);
-}
-// > LAND
-void MainGUIWindow::on_all_land_button_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_CRAZYFLY_LAND;
- commandAllAgentsPublisher.publish(msg);
-}
-// > MOTORS OFF
-void MainGUIWindow::on_all_motors_off_button_clicked()
-{
- std_msgs::Int32 msg;
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
msg.data = CMD_CRAZYFLY_MOTORS_OFF;
- commandAllAgentsPublisher.publish(msg);
- //emergencyStopPublisher.publish(msg);
-}
-// > ENABLE SAFE CONTROLLER
-void MainGUIWindow::on_all_enable_safe_controller_button_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_USE_SAFE_CONTROLLER;
- commandAllAgentsPublisher.publish(msg);
-}
-// > ENABLE SAFE CONTROLLER
-void MainGUIWindow::on_all_enable_custom_controller_button_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_USE_CUSTOM_CONTROLLER;
- commandAllAgentsPublisher.publish(msg);
-}
-// > LOAD THE YAML PARAMETERS FOR THE SAFE CONTROLLER
-void MainGUIWindow::on_all_load_safe_controller_yaml_own_agent_button_clicked()
-{
- // Disable the button
- ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(false);
- ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(false);
-
- // Send the message that the YAML paremters should be loaded
- std_msgs::Int32 msg;
- msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
- requestLoadControllerYamlPublisher.publish(msg);
-
- // Start a timer which will enable the button in its callback
- // > This is required because the agent node waits some time between
- // re-loading the values from the YAML file and then assigning then
- // to the local variable of the agent.
- // > Thus we use this timer to prevent the user from clicking the
- // button in the GUI repeatedly.
- ros::NodeHandle nodeHandle("~");
- m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::safeYamlFileTimerCallback, this, true);
-}
-// > LOAD THE YAML PARAMETERS FOR THE CUSTOM CONTROLLER
-void MainGUIWindow::on_all_load_custom_controller_yaml_own_agent_button_clicked()
-{
- // Disable the button
- ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(false);
- ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(false);
-
- // Send the message that the YAML paremters should be loaded
- std_msgs::Int32 msg;
- msg.data = LOAD_YAML_CUSTOM_CONTROLLER_AGENT;
- requestLoadControllerYamlPublisher.publish(msg);
-
- // Start a timer which will enable the button in its callback
- // > This is required because the agent node waits some time between
- // re-loading the values from the YAML file and then assigning then
- // to the local variable of the agent.
- // > Thus we use this timer to prevent the user from clicking the
- // button in the GUI repeatedly.
- ros::NodeHandle nodeHandle("~");
- m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
-
-}
-// > SEND THE YAML PARAMETERS FOR THE SAFE CONTROLLER
-void MainGUIWindow::on_all_load_safe_controller_yaml_coordinator_button_clicked()
-{
- // Disable the button
- ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(false);
- ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(false);
-
- // Send the message that the YAML paremters should be loaded
- // by the coordinator (and then the agent informed)
- std_msgs::Int32 msg;
- msg.data = LOAD_YAML_SAFE_CONTROLLER_COORDINATOR;
- requestLoadControllerYamlPublisher.publish(msg);
-
- // Start a timer which will enable the button in its callback
- // > This is required because the agent node waits some time between
- // re-loading the values from the YAML file and then assigning then
- // to the local variable of the agent.
- // > Thus we use this timer to prevent the user from clicking the
- // button in the GUI repeatedly.
- ros::NodeHandle nodeHandle("~");
- m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::safeYamlFileTimerCallback, this, true);
-}
-// > SEND THE YAML PARAMETERS FOR THE CUSTOM CONTROLLER
-void MainGUIWindow::on_all_load_custom_controller_yaml_coordinator_button_clicked()
-{
- // Disable the button
- ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(false);
- ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(false);
-
- // Send the message that the YAML paremters should be loaded
- // by the coordinator (and then the agent informed)
- std_msgs::Int32 msg;
- msg.data = LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR;
- requestLoadControllerYamlPublisher.publish(msg);
-
- // Start a timer which will enable the button in its callback
- // > This is required because the agent node waits some time between
- // re-loading the values from the YAML file and then assigning then
- // to the local variable of the agent.
- // > Thus we use this timer to prevent the user from clicking the
- // button in the GUI repeatedly.
- ros::NodeHandle nodeHandle("~");
- m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
-
-}
-// > CALLBACK TO RE-ENABLE THE SAFE CONTROLLER YAML BUTTONS
-void MainGUIWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
-{
- // Enble the "load" and the "send" safe controller YAML button again
- ui->all_load_safe_controller_yaml_own_agent_button->setEnabled(true);
- ui->all_load_safe_controller_yaml_coordinator_button->setEnabled(true);
-}
-// > CALLBACK TO RE-ENABLE THE CUSTOM CONTROLLER YAML BUTTONS
-void MainGUIWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
-{
- // Enble the "load" and the "send" custom controller YAML button again
- ui->all_load_custom_controller_yaml_own_agent_button->setEnabled(true);
- ui->all_load_custom_controller_yaml_coordinator_button->setEnabled(true);
-}
-
-/*
-// > CALLBACK TO SEND THE CUSTOM CONTROLLER YAML PARAMETERS AS A MESSAGE
-void MainGUIWindow::customSendYamlAsMessageTimerCallback(const ros::TimerEvent&)
-{
- // Load the CUSTOM controller YAML parameters from file into a message for
- // sending directly to the "CustonControllerService" node of every agent
-
- ros::NodeHandle nodeHandle("~");
-
- // Instaniate a local variable of the message type
- CustomControllerYAML customControllerYamlMessage;
-
- // Load the data directly from the YAML file into the message
-
- // > For the mass
- customControllerYamlMessage.mass = MainGUIWindow::getParameterFloat(nodeHandle, "mass");
-
- // Debugging this this works
- ROS_INFO_STREAM("DEBUGGING: mass loaded from the custom controller YAML = " << customControllerYamlMessage.mass );
-
- // > For the control_frequency
- customControllerYamlMessage.control_frequency = MainGUIWindow::getParameterFloat(nodeHandle, "control_frequency");
-
- // > For the motorPoly coefficients
- std::vector<float> temp_motorPoly(3);
- MainGUIWindow::getParameterFloatVector(nodeHandle, "motorPoly", temp_motorPoly, 3);
- // Copy the loaded data into the message
- for ( int i=0 ; i<3 ; i++ )
- {
- customControllerYamlMessage.motorPoly.push_back( temp_motorPoly[i] );
- }
-
- // > For the boolean about whether to execute the convert into body frame function
- customControllerYamlMessage.shouldPerformConvertIntoBodyFrame = MainGUIWindow::getParameterBool(nodeHandle, "shouldPerformConvertIntoBodyFrame");
-
- // > For the boolean about publishing the agents current position
- customControllerYamlMessage.shouldPublishCurrent_xyz_yaw = MainGUIWindow::getParameterBool(nodeHandle, "shouldPublishCurrent_xyz_yaw");
-
- // > For the boolean about following another agent
- customControllerYamlMessage.shouldFollowAnotherAgent = MainGUIWindow::getParameterBool(nodeHandle, "shouldFollowAnotherAgent");
-
- // > For the ordered agent ID's for following eachother in a line
- std::vector<int> temp_follow_in_a_line_agentIDs(100);
- int temp_number_of_agents_in_a_line = MainGUIWindow::getParameterIntVectorWithUnknownLength(nodeHandle, "follow_in_a_line_agentIDs", temp_follow_in_a_line_agentIDs);
- // > Double check that the sizes agree
- if ( temp_number_of_agents_in_a_line != temp_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 " << temp_follow_in_a_line_agentIDs.size() );
- }
- // Copy the loaded data into the message
- for ( int i=0 ; i<temp_number_of_agents_in_a_line ; i++ )
- {
- customControllerYamlMessage.follow_in_a_line_agentIDs.push_back( temp_follow_in_a_line_agentIDs[i] );
- }
-
- // Publish the message containing the loaded YAML parameters
- customYAMLasMessagePublisher.publish(customControllerYamlMessage);
-
- // Start a timer which will enable the button in its callback
- m_timer_yaml_file_for_custom_controller = nodeHandle.createTimer(ros::Duration(0.5), &MainGUIWindow::customYamlFileTimerCallback, this, true);
-}
-*/
-
-
-
-
-// ----------------------------------------------------------------------------------
-// L OOO A DDDD Y Y A M M L
-// L O O A A D D Y Y A A MM MM L
-// L O O A A D D Y A A M M M L
-// L O O AAAAA D D Y AAAAA M M L
-// LLLLL OOO A A DDDD Y A A M M LLLLL
-//
-// 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
-// ----------------------------------------------------------------------------------
-
-// load parameters from corresponding YAML file
-//
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-float MainGUIWindow::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 MainGUIWindow::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 MainGUIWindow::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 MainGUIWindow::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 MainGUIWindow::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 MainGUIWindow::getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
-{
- bool val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
+ msg.shouldCheckForAgentID = false;
+ emergencyStopPublisher.publish(msg);
+#endif
}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/marker.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/marker.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/marker.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/marker.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/myGraphicsRectItem.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/myGraphicsRectItem.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/myGraphicsRectItem.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/myGraphicsRectItem.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/myGraphicsScene.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/myGraphicsScene.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/myGraphicsScene.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/myGraphicsScene.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/myGraphicsView.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/myGraphicsView.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/myGraphicsView.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/myGraphicsView.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/rosNodeThread_for_managerGUI.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/rosNodeThread_for_systemConfigGUI.cpp
similarity index 82%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/rosNodeThread_for_managerGUI.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/rosNodeThread_for_systemConfigGUI.cpp
index cae1ed0efaae8a4b3cdc13b41e838080ab6eb344..847e86160f63d1931c4da8e62ec022dc04f15b70 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/rosNodeThread_for_managerGUI.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/rosNodeThread_for_systemConfigGUI.cpp
@@ -30,11 +30,11 @@
// ----------------------------------------------------------------------------------
-#include "rosNodeThread_for_managerGUI.h"
+#include "rosNodeThread_for_systemConfigGUI.h"
-#include "d_fall_pps/CMRead.h"
-#include "d_fall_pps/CMUpdate.h"
-#include "d_fall_pps/CMCommand.h"
+#include "dfall_pkg/CMRead.h"
+#include "dfall_pkg/CMUpdate.h"
+#include "dfall_pkg/CMCommand.h"
rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name, QObject* parent)
@@ -63,7 +63,9 @@ bool rosNodeThread::init()
this->moveToThread(m_pThread); // QObject method
connect(m_pThread, SIGNAL(started()), this, SLOT(run()));
- ros::init(m_Init_argc, m_pInit_argv, m_node_name); // my_GUI is the name of this node
+ ros::init(m_Init_argc, m_pInit_argv, m_node_name);
+ // Note that the variable "m_node_name" should be the
+ // string "SystemConfigGUI" in this case
if (!ros::master::check())
{
@@ -75,12 +77,14 @@ bool rosNodeThread::init()
ros::Time::init();
ros::NodeHandle nh("~");
- m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+ m_vicon_subscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
// clients for db services:
- m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
- m_update_db_client = nh.serviceClient<CMUpdate>("/CentralManagerService/Update", false);
- m_command_db_client = nh.serviceClient<CMCommand>("/CentralManagerService/Command", false);
+ m_read_db_client = nodeHandle_dfall_root.serviceClient<CMRead>("CentralManagerService/Read", false);
+ m_update_db_client = nodeHandle_dfall_root.serviceClient<CMUpdate>("CentralManagerService/Update", false);
+ m_command_db_client = nodeHandle_dfall_root.serviceClient<CMCommand>("CentralManagerService/Command", false);
m_pThread->start();
return true;
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/tablePiece.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/tablePiece.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/tablePiece.cpp
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/tablePiece.cpp
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/systemConfigGUI.pro
similarity index 77%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/systemConfigGUI.pro
index 1f9cbd2f3351c91ff071eb34291805de96c1d7a5..4c480d64f3a9fc976d0561e1e3ab40f308690a29 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/systemConfigGUI.pro
@@ -8,18 +8,20 @@ QT += core gui
greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
-TARGET = CrazyFlyGUI
+TARGET = systemConfigGUI
TEMPLATE = app
INCLUDEPATH += $$PWD/include
CONFIG += c++11
-RESOURCES = CrazyFlyGUI.qrc
+RESOURCES = systemconfiggui.qrc
QT+= svg
SOURCES += \
+ src/centerMarker.cpp \
+ src/channelLUT.cpp \
src/cornergrabber.cpp \
src/crazyFlyZone.cpp \
src/crazyFlyZoneTab.cpp \
@@ -31,6 +33,8 @@ SOURCES += \
src/tablePiece.cpp
HEADERS += \
+ include/centerMarker.h \
+ include/channelLUT.h \
include/cornergrabber.h \
include/crazyFlyZone.h \
include/crazyFlyZoneTab.h \
@@ -41,7 +45,9 @@ HEADERS += \
include/tablePiece.h \
include/globalDefinitions.h \
include/marker.h \
- include/crazyFly.h
+ include/crazyFly.h \
+ \
+ include/constants_for_qt_compile.h
FORMS += \
- src/mainguiwindow.ui
+ forms/mainguiwindow.ui
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.qrc b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/systemconfiggui.qrc
similarity index 100%
rename from pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.qrc
rename to dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/systemconfiggui.qrc
diff --git a/pps_ws/src/d_fall_pps/PPS.perspective b/dfall_ws/src/dfall_pkg/PPS.perspective
similarity index 98%
rename from pps_ws/src/d_fall_pps/PPS.perspective
rename to dfall_ws/src/dfall_pkg/PPS.perspective
index 57c0387a25b45c53b9f8aee97055105dff1c6601..87d70db1377709a435e0616799862ef1f3ff7835 100644
--- a/pps_ws/src/d_fall_pps/PPS.perspective
+++ b/dfall_ws/src/dfall_pkg/PPS.perspective
@@ -191,7 +191,7 @@
"keys": {
"publishers": {
"type": "repr",
- "repr": "u\"[{'type_name': 'std_msgs/Int32', 'topic_name': '/PPSClient/Command', 'enabled': False, 'rate': 1.0, 'expressions': {u'/PPSClient/Command/data': '3'}, 'publisher_id': 0, 'counter': 0}]\""
+ "repr": "u\"[{'type_name': 'std_msgs/Int32', 'topic_name': '/FlyingAgentClient/Command', 'enabled': False, 'rate': 1.0, 'expressions': {u'/FlyingAgentClient/Command/data': '3'}, 'publisher_id': 0, 'counter': 0}]\""
}
},
"groups": {}
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
similarity index 61%
rename from pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
rename to dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
index 8562b558a21c7446598ecffaaa02bea001f802de..3345d60630b2e7832208a8a8eda1989d4c244cd7 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
@@ -33,10 +33,12 @@
# ----------------------------------------------------------------------------------
-import roslib; roslib.load_manifest('d_fall_pps')
+import roslib; roslib.load_manifest('dfall_pkg')
import rospy
-from d_fall_pps.msg import ControlCommand
from std_msgs.msg import Int32
+from dfall_pkg.msg import ControlCommand
+from dfall_pkg.msg import IntWithHeader
+from dfall_pkg.srv import IntIntService
# General import
@@ -70,10 +72,9 @@ logging.basicConfig(level=logging.ERROR)
# CONTROLLER_ANGLE = 1
# CONTROLLER_RATE = 0
-
-TYPE_PPS_MOTORS = 6
-TYPE_PPS_RATE = 7
-TYPE_PPS_ANGLE = 8
+CF_COMMAND_TYPE_MOTORS = 6
+CF_COMMAND_TYPE_RATE = 7
+CF_COMMAND_TYPE_ANGLE = 8
RAD_TO_DEG = 57.296
@@ -86,7 +87,7 @@ DISCONNECTED = 2
CMD_RECONNECT = 0
CMD_DISCONNECT = 1
-# Commands for PPSClient
+# Commands for FlyingAgentClient
#CMD_USE_SAFE_CONTROLLER = 1
#CMD_USE_DEMO_CONTROLLER = 2
#CMD_USE_STUDENT_CONTROLLER = 3
@@ -99,12 +100,12 @@ CMD_CRAZYFLY_LAND = 12
CMD_CRAZYFLY_MOTORS_OFF = 13
rp = RosPack()
-record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
+record_file = rp.get_path('dfall_pkg') + '/LoggingOnboard.bag'
rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
rospy.loginfo(record_file)
bag = rosbag.Bag(record_file, 'w')
-class PPSRadioClient:
+class CrazyRadioClient:
"""
CrazyRadio client that recieves the commands from the controller and
sends them in a CRTP package to the crazyflie with the specified
@@ -124,7 +125,7 @@ class PPSRadioClient:
self.link_uri = ""
self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
- self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', Int32, queue_size=1)
+ self.FlyingAgentClient_command_pub = rospy.Publisher('FlyingAgentClient/Command', IntWithHeader, queue_size=1)
time.sleep(1.0)
# Initialize the CrazyFlie and add callbacks
@@ -145,7 +146,7 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED)
def change_status_to(self, new_status):
- print "status changed to: %s" % new_status
+ print "[CRAZY RADIO] status changed to: %s" % new_status
self._status = new_status
self.status_pub.publish(new_status)
@@ -159,18 +160,21 @@ class PPSRadioClient:
# update link from ros params
self.update_link_uri()
- print "Connecting to %s" % self.link_uri
+ print "[CRAZY RADIO] Connecting to %s" % self.link_uri
self.change_status_to(CONNECTING)
- rospy.loginfo("connecting...")
+ rospy.loginfo("[CRAZY RADIO] connecting...")
self._cf.open_link(self.link_uri)
def disconnect(self):
- print "Motors OFF"
+ print "[CRAZY RADIO] sending Motors OFF command before disconnecting"
self._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
- self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+ msg = IntWithHeader()
+ msg.shouldCheckForAgentID = False
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF
+ self.FlyingAgentClient_command_pub.publish(msg)
time.sleep(0.1)
- print "Disconnecting from %s" % self.link_uri
+ print "[CRAZY RADIO] Disconnecting from %s" % self.link_uri
self._cf.close_link()
self.change_status_to(DISCONNECTED)
@@ -196,7 +200,7 @@ class PPSRadioClient:
def _logging_error(self, logconf, msg):
- print "Error when logging %s" % logconf.name
+ print "[CRAZY RADIO] Error when logging %s" % logconf.name
# def _init_logging(self):
@@ -211,12 +215,12 @@ class PPSRadioClient:
if self.logconf.valid:
self.logconf.data_received_cb.add_callback(self._data_received_callback)
self.logconf.error_cb.add_callback(self._logging_error)
- print "logconf valid"
+ print "[CRAZY RADIO] logconf valid"
else:
- print "logconf invalid"
+ print "[CRAZY RADIO] logconf invalid"
self.logconf.start()
- print "logconf start"
+ print "[CRAZY RADIO] logconf start"
def _connected(self, link_uri):
"""
@@ -225,9 +229,12 @@ class PPSRadioClient:
"""
self.change_status_to(CONNECTED)
# change state to motors OFF
- cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+ msg = IntWithHeader()
+ msg.shouldCheckForAgentID = False
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF
+ cf_client.FlyingAgentClient_command_pub.publish(msg)
- rospy.loginfo("Connection to %s successful: " % link_uri)
+ rospy.loginfo("[CRAZY RADIO] Connection to %s successful: " % link_uri)
# Config for Logging
self._start_logging()
@@ -238,21 +245,24 @@ class PPSRadioClient:
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
self.change_status_to(DISCONNECTED)
- rospy.logerr("Connection to %s failed: %s" % (link_uri, msg))
+ rospy.logerr("[CRAZY RADIO] Connection to %s failed: %s" % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
self.change_status_to(DISCONNECTED)
- rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
+ rospy.logerr("[CRAZY RADIO] Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
self.change_status_to(DISCONNECTED)
- rospy.logwarn("Disconnected from %s" % link_uri)
+ rospy.logwarn("[CRAZY RADIO] Disconnected from %s" % link_uri)
# change state to motors OFF
- self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+ msg = IntWithHeader()
+ msg.shouldCheckForAgentID = False
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF
+ self.FlyingAgentClient_command_pub.publish(msg)
self.logconf.stop()
rospy.loginfo("logconf stopped")
@@ -262,19 +272,19 @@ class PPSRadioClient:
def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHH', TYPE_PPS_MOTORS, cmd1, cmd2, cmd3, cmd4)
+ pk.data = struct.pack('<BHHHH', CF_COMMAND_TYPE_MOTORS, cmd1, cmd2, cmd3, cmd4)
self._cf.send_packet(pk)
def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHHfff', TYPE_PPS_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate * RAD_TO_DEG, pitch_rate * RAD_TO_DEG, yaw_rate * RAD_TO_DEG)
+ pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate * RAD_TO_DEG, pitch_rate * RAD_TO_DEG, yaw_rate * RAD_TO_DEG)
self._cf.send_packet(pk)
def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHHfff', TYPE_PPS_ANGLE, cmd1, cmd2, cmd3, cmd4, roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG)
+ pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2, cmd3, cmd4, roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG)
self._cf.send_packet(pk)
# def _send_to_commander(self,roll, pitch, yaw, thrust, cmd1, cmd2, cmd3, cmd4, mode):
@@ -285,23 +295,53 @@ class PPSRadioClient:
def crazyRadioCommandCallback(self, msg):
"""Callback to tell CrazyRadio to reconnect"""
- print "crazyRadio command received %s" % msg.data
-
- if msg.data == CMD_RECONNECT: # reconnect, check _status first and then do whatever needs to be done
- print "entered reconnect"
- print "_status: %s" % self._status
- if self.get_status() == DISCONNECTED:
- print "entered disconnected"
- self.connect()
- if self.get_status() == CONNECTED:
- self.status_pub.publish(CONNECTED)
-
- elif msg.data == CMD_DISCONNECT:
- print "disconnect received"
- if self.get_status() != DISCONNECTED: # what happens if we disconnect while we are in connecting state?
- self.disconnect()
- else:
- self.status_pub.publish(DISCONNECTED)
+
+ # Initialise a boolean flag that the command is NOT relevant
+ command_is_relevant = False
+
+ # Check the header details of the message for it relevance
+ if (msg.shouldCheckForAgentID == False):
+ command_is_relevant = True
+ else:
+ for this_ID in msg.agentIDs:
+ if (this_ID == m_agentID):
+ command_is_relevant = True
+ break
+
+ # Only consider the command if it is relevant
+ if (command_is_relevant):
+ #print "[CRAZY RADIO] received command to: %s" % msg.data
+ if msg.data == CMD_RECONNECT:
+ if self.get_status() == DISCONNECTED:
+ print "[CRAZY RADIO] received command to CONNECT (current status is DISCONNECTED)"
+ self.connect()
+ elif self.get_status() == CONNECTING:
+ print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTING)"
+ #self.status_pub.publish(CONNECTING)
+ elif self.get_status() == CONNECTED:
+ print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTED)"
+ #self.status_pub.publish(CONNECTED)
+
+ elif msg.data == CMD_DISCONNECT:
+ if self.get_status() == CONNECTED:
+ print "[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTED)"
+ self.disconnect()
+ elif self.get_status() == CONNECTING:
+ print "[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTING)"
+ #self.status_pub.publish(CONNECTING)
+ elif self.get_status() == DISCONNECTED:
+ print "[CRAZY RADIO] received command to DISCONNECT (current status is DISCONNECTED)"
+ #self.status_pub.publish(DISCONNECTED)
+
+
+
+ def getCurrentCrazyRadioStatusServiceCallback(self, req):
+ """Callback to service the request for the connection status"""
+ # Directly return the current status
+ return self._status
+
+
+
def controlCommandCallback(data):
"""Callback for controller actions"""
@@ -309,63 +349,101 @@ def controlCommandCallback(data):
#cmd1..4 must not be 0, as crazyflie onboard controller resets!
#pitch and yaw are inverted on crazyflie controller
- if data.onboardControllerType == TYPE_PPS_MOTORS:
+ if data.onboardControllerType == CF_COMMAND_TYPE_MOTORS:
cf_client._send_to_commander_motor(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)
- elif data.onboardControllerType == TYPE_PPS_RATE:
+ elif data.onboardControllerType == CF_COMMAND_TYPE_RATE:
cf_client._send_to_commander_rate(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
- elif data.onboardControllerType == TYPE_PPS_ANGLE:
+ elif data.onboardControllerType == CF_COMMAND_TYPE_ANGLE:
cf_client._send_to_commander_angle(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
# cf_client._send_to_commander(data.roll, -data.pitch, -data.yaw, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
+
+
if __name__ == '__main__':
+
+ # Starting the ROS-node
global node_name
node_name = "CrazyRadio"
rospy.init_node(node_name, anonymous=True)
+ # Get the namespace of this node
global ros_namespace
ros_namespace = rospy.get_namespace()
+
+
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
- #wait until address parameter is set by PPSClient
+ #wait until address parameter is set by FlyingAgentClient
while not rospy.has_param("~crazyFlieAddress"):
time.sleep(0.05)
#use this following two lines to connect without data from CentralManager
# radio_address = "radio://0/72/2M"
# rospy.loginfo("manual address loaded")
+
+ # Fetch the YAML paramter "battery polling period"
global battery_polling_period
battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")
+ # Fetch the YAML paramter "agentID" and "coordID"
+ global m_agentID
+ m_agentID = rospy.get_param(ros_namespace + "/FlyingAgentClient/agentID")
+ coordID = rospy.get_param(ros_namespace + "/FlyingAgentClient/coordID")
+ # Convert the coordinator ID to a zero-padded string
+ coordID_as_string = format(coordID, '03')
+
+
+ # Initialise a publisher for the battery voltage
global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
+ # Initialise a "CrazyRadioClient" type variable that handles
+ # all communication over the CrazyRadio
global cf_client
+ cf_client = CrazyRadioClient()
+
+ print "[CRAZY RADIO] DEBUG 2"
+
+ # Subscribe to the commands for when to (dis-)connect the
+ # CrazyRadio connection with the Crazyflie
+ # > For the radio commands from the FlyingAgentClient of this agent
+ rospy.Subscriber("FlyingAgentClient/crazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback)
+ # > For the radio command from the coordinator
+ rospy.Subscriber("/dfall/coord" + coordID_as_string + "/FlyingAgentClient/crazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback)
+
+
+ # Advertise a Serice for the current status
+ # of the Crazy Radio connect
+ s = rospy.Service(node_name + "/getCurrentCrazyRadioStatus", IntIntService , cf_client.getCurrentCrazyRadioStatusServiceCallback)
+
- cf_client = PPSRadioClient()
- rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
time.sleep(1.0)
- rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
+ rospy.Subscriber("FlyingAgentClient/ControlCommand", ControlCommand, controlCommandCallback)
+ print "[CRAZY RADIO] Node READY :-)"
rospy.spin()
- rospy.loginfo("Turning off crazyflie")
+ rospy.loginfo("[CRAZY RADIO] Turning off crazyflie")
cf_client._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
- cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
+ msg = IntWithHeader()
+ msg.shouldCheckForAgentID = False
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF
+ cf_client.FlyingAgentClient_command_pub.publish(msg)
#wait for client to send its commands
time.sleep(1.0)
bag.close()
- rospy.loginfo("bag closed")
+ rospy.loginfo("[CRAZY RADIO] bag closed")
cf_client._cf.close_link()
- rospy.loginfo("Link closed")
+ rospy.loginfo("[CRAZY RADIO] Link closed")
diff --git a/pps_ws/src/d_fall_pps/crazyradio/LICENSE.txt b/dfall_ws/src/dfall_pkg/crazyradio/LICENSE.txt
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/LICENSE.txt
rename to dfall_ws/src/dfall_pkg/crazyradio/LICENSE.txt
diff --git a/pps_ws/src/d_fall_pps/crazyradio/Makefile b/dfall_ws/src/dfall_pkg/crazyradio/Makefile
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/Makefile
rename to dfall_ws/src/dfall_pkg/crazyradio/Makefile
diff --git a/pps_ws/src/d_fall_pps/crazyradio/README.md b/dfall_ws/src/dfall_pkg/crazyradio/README.md
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/README.md
rename to dfall_ws/src/dfall_pkg/crazyradio/README.md
diff --git a/pps_ws/src/d_fall_pps/crazyradio/TestCF.py b/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
similarity index 84%
rename from pps_ws/src/d_fall_pps/crazyradio/TestCF.py
rename to dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
index c5fa4f44fad954a0c86f728de59046aade762bd3..f955bd2c0388996340f9729b490474e10be1aa58 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/TestCF.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
@@ -1,9 +1,9 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
-# import roslib; roslib.load_manifest('d_fall_pps')
+# import roslib; roslib.load_manifest('dfall_pkg')
# import rospy
-# from d_fall_pps.msg import ControlCommand
+# from dfall_pkg.msg import ControlCommand
# from std_msgs.msg import Int32
@@ -36,9 +36,9 @@ logging.basicConfig(level=logging.ERROR)
# Types:
-TYPE_PPSMOTORS = 6
-TYPE_PPSRATE = 7
-TYPE_PPSANGLE = 8
+CF_COMMAND_TYPE_MOTORS = 6
+CF_COMMAND_TYPE_RATE = 7
+CF_COMMAND_TYPE_ANGLE = 8
CONTROLLER_MOTOR = 2
CONTROLLER_ANGLE = 1
@@ -54,20 +54,14 @@ DISCONNECTED = 2
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
# rp = RosPack()
-# record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
+# record_file = rp.get_path('dfall_pkg') + '/LoggingOnboard.bag'
# rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
# rospy.loginfo(record_file)
# bag = rosbag.Bag(record_file, 'w')
-class PPSRadioClient:
+class CrazyRadioClient:
"""
CrazyRadio client that recieves the commands from the controller and
sends them in a CRTP package to the crazyflie with the specified
@@ -87,7 +81,7 @@ class PPSRadioClient:
self.link_uri = ""
# self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
- # self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', Int32, queue_size=1)
+ # self.FlyingAgentClient_command_pub = rospy.Publisher('FlyingAgentClient/Command', Int32, queue_size=1)
time.sleep(1.0)
# Initialize the CrazyFlie and add callbacks
@@ -192,19 +186,19 @@ class PPSRadioClient:
def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHH', TYPE_PPSMOTORS, cmd1, cmd2, cmd3, cmd4)
+ pk.data = struct.pack('<BHHHH', CF_COMMAND_TYPE_MOTORS, cmd1, cmd2, cmd3, cmd4)
self._cf.send_packet(pk)
def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHHfff', TYPE_PPSRATE, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate)
+ pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate)
self._cf.send_packet(pk)
def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHHfff', TYPE_PPSANGLE, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw)
+ pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw)
self._cf.send_packet(pk)
if __name__ == '__main__':
@@ -215,7 +209,7 @@ if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
- #wait until address parameter is set by PPSClient
+ #wait until address parameter is set by FlyingAgentClient
# while not rospy.has_param("~crazyFlieAddress"):
# time.sleep(0.05)
@@ -228,19 +222,17 @@ if __name__ == '__main__':
global cf_client
- cf_client = PPSRadioClient()
- # rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
+ cf_client = CrazyRadioClient()
+ # rospy.Subscriber("FlyingAgentClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
# time.sleep(1.0)
- # rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)
+ # rospy.Subscriber("FlyingAgentClient/ControlCommand", ControlCommand, controlCommandCallback)
# rospy.spin()
# rospy.loginfo("Turning off crazyflie")
- # change state to motors OFF
- # cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
#wait for client to send its commands
# time.sleep(1.0)
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/bootloader/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/bootloader/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/bootloader/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/bootloader/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/bootloader/boottypes.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/bootloader/boottypes.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/bootloader/boottypes.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/bootloader/boottypes.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/bootloader/cloader.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/bootloader/cloader.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/bootloader/cloader.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/bootloader/cloader.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/commander.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/commander.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/commander.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/commander.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/console.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/console.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/console.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/console.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/extpos.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/extpos.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/extpos.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/extpos.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/localization.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/localization.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/localization.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/localization.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/log.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/log.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/log.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/log.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/mem.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/mem.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/mem.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/mem.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/param.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/param.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/param.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/param.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/platformservice.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/platformservice.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/platformservice.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/platformservice.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/swarm.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/swarm.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/swarm.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/swarm.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/syncCrazyflie.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/syncCrazyflie.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/syncCrazyflie.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/syncCrazyflie.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/syncLogger.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/syncLogger.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/syncLogger.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/syncLogger.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/toc.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/toc.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/toc.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/toc.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/toccache.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/toccache.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/toccache.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crazyflie/toccache.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/crtpdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/crtpdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/crtpdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/crtpdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/crtpstack.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/crtpstack.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/crtpstack.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/crtpstack.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/debugdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/debugdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/debugdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/debugdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/exceptions.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/exceptions.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/exceptions.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/exceptions.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/radiodriver.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/radiodriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/radiodriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/radiodriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/serialdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/serialdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/serialdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/serialdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/udpdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/udpdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/udpdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/udpdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/usbdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/usbdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crtp/usbdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/crtp/usbdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/drivers/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/drivers/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/drivers/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/drivers/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/drivers/cfusb.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/drivers/cfusb.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/drivers/cfusb.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/drivers/cfusb.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/drivers/crazyradio.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/drivers/crazyradio.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/drivers/crazyradio.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/drivers/crazyradio.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/utils/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/utils/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/utils/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/utils/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/utils/callbacks.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/utils/callbacks.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/utils/callbacks.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/cflib/utils/callbacks.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/lpslib/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/lpslib/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/lpslib/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/lpslib/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/lpslib/lopoanchor.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/lpslib/lopoanchor.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/lpslib/lopoanchor.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/lpslib/lopoanchor.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crazyflie/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crazyflie/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crazyflie/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crazyflie/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crazyflie/test_swarm.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crazyflie/test_swarm.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crazyflie/test_swarm.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crazyflie/test_swarm.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crazyflie/test_syncCrazyflie.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crazyflie/test_syncCrazyflie.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crazyflie/test_syncCrazyflie.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crazyflie/test_syncCrazyflie.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crazyflie/test_syncLogger.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crazyflie/test_syncLogger.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crazyflie/test_syncLogger.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crazyflie/test_syncLogger.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crtp/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crtp/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crtp/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crtp/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crtp/test_crtpstack.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crtp/test_crtpstack.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/crtp/test_crtpstack.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/crtp/test_crtpstack.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/support/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/support/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/support/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/support/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/support/asyncCallbackCaller.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/support/asyncCallbackCaller.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/support/asyncCallbackCaller.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/support/asyncCallbackCaller.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/utils/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/utils/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/utils/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/utils/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/build/lib/test/utils/test_callbacks.py b/dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/utils/test_callbacks.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/build/lib/test/utils/test_callbacks.py
rename to dfall_ws/src/dfall_pkg/crazyradio/build/lib/test/utils/test_callbacks.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/PKG-INFO b/dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/PKG-INFO
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/PKG-INFO
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/PKG-INFO
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/SOURCES.txt b/dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/SOURCES.txt
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/SOURCES.txt
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/SOURCES.txt
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/dependency_links.txt b/dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/dependency_links.txt
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/dependency_links.txt
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/dependency_links.txt
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/requires.txt b/dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/requires.txt
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/requires.txt
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/requires.txt
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/top_level.txt b/dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/top_level.txt
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib.egg-info/top_level.txt
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib.egg-info/top_level.txt
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/bootloader/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/bootloader/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/boottypes.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/bootloader/boottypes.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/boottypes.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/bootloader/boottypes.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/bootloader/cloader.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/bootloader/cloader.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/commander.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/commander.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/console.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/console.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/extpos.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/extpos.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/extpos.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/extpos.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/localization.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/localization.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/localization.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/localization.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/log.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/log.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/mem.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/mem.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/param.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/param.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/platformservice.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/platformservice.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/swarm.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/swarm.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/swarm.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/swarm.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/syncCrazyflie.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/syncCrazyflie.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/syncCrazyflie.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/syncCrazyflie.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/syncLogger.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/syncLogger.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/syncLogger.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/syncLogger.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/toc.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/toc.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/toccache.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crazyflie/toccache.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/crtpdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/crtpdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/crtpstack.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/crtpstack.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/debugdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/debugdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/exceptions.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/exceptions.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/radiodriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/radiodriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/serialdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/serialdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/udpdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/udpdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/usbdriver.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/crtp/usbdriver.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/drivers/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/drivers/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/drivers/cfusb.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/drivers/cfusb.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/drivers/crazyradio.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/drivers/crazyradio.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/utils/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/utils/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.py b/dfall_ws/src/dfall_pkg/crazyradio/cflib/utils/callbacks.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.py
rename to dfall_ws/src/dfall_pkg/crazyradio/cflib/utils/callbacks.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/dist/cflib-0.1.3-py3.5.egg b/dfall_ws/src/dfall_pkg/crazyradio/dist/cflib-0.1.3-py3.5.egg
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/dist/cflib-0.1.3-py3.5.egg
rename to dfall_ws/src/dfall_pkg/crazyradio/dist/cflib-0.1.3-py3.5.egg
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/autonomousSequence.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/autonomousSequence.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/autonomousSequence.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/autonomousSequence.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/basiclog.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/basiclog.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/basiclog.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/basiclog.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/basiclogSync.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/basiclogSync.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/basiclogSync.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/basiclogSync.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/basicparam.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/basicparam.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/basicparam.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/basicparam.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/erase-ow.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/erase-ow.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/erase-ow.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/erase-ow.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/flash-memory.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/flash-memory.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/flash-memory.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/flash-memory.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/flowsequenceSync.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/flowsequenceSync.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/flowsequenceSync.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/flowsequenceSync.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/lps_reboot_to_bootloader.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/lps_reboot_to_bootloader.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/lps_reboot_to_bootloader.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/lps_reboot_to_bootloader.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/multiramp.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/multiramp.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/multiramp.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/multiramp.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/radio-test.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/radio-test.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/radio-test.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/radio-test.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/ramp.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/ramp.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/ramp.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/ramp.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/read-eeprom.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/read-eeprom.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/read-eeprom.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/read-eeprom.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/read-ow.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/read-ow.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/read-ow.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/read-ow.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/scan.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/scan.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/scan.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/scan.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/swarmSequence.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/swarmSequence.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/swarmSequence.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/swarmSequence.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/swarmSequenceCircle.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/swarmSequenceCircle.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/swarmSequenceCircle.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/swarmSequenceCircle.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/write-eeprom.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/write-eeprom.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/write-eeprom.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/write-eeprom.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/examples/write-ow.py b/dfall_ws/src/dfall_pkg/crazyradio/examples/write-ow.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/examples/write-ow.py
rename to dfall_ws/src/dfall_pkg/crazyradio/examples/write-ow.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/lpslib/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/lpslib/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/lpslib/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/lpslib/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/lpslib/lopoanchor.py b/dfall_ws/src/dfall_pkg/crazyradio/lpslib/lopoanchor.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/lpslib/lopoanchor.py
rename to dfall_ws/src/dfall_pkg/crazyradio/lpslib/lopoanchor.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/module.json b/dfall_ws/src/dfall_pkg/crazyradio/module.json
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/module.json
rename to dfall_ws/src/dfall_pkg/crazyradio/module.json
diff --git a/pps_ws/src/d_fall_pps/crazyradio/requirements-dev.txt b/dfall_ws/src/dfall_pkg/crazyradio/requirements-dev.txt
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/requirements-dev.txt
rename to dfall_ws/src/dfall_pkg/crazyradio/requirements-dev.txt
diff --git a/pps_ws/src/d_fall_pps/crazyradio/requirements.txt b/dfall_ws/src/dfall_pkg/crazyradio/requirements.txt
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/requirements.txt
rename to dfall_ws/src/dfall_pkg/crazyradio/requirements.txt
diff --git a/pps_ws/src/d_fall_pps/crazyradio/setup.cfg b/dfall_ws/src/dfall_pkg/crazyradio/setup.cfg
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/setup.cfg
rename to dfall_ws/src/dfall_pkg/crazyradio/setup.cfg
diff --git a/pps_ws/src/d_fall_pps/crazyradio/setup.py b/dfall_ws/src/dfall_pkg/crazyradio/setup.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/setup.py
rename to dfall_ws/src/dfall_pkg/crazyradio/setup.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/setup_linux.sh b/dfall_ws/src/dfall_pkg/crazyradio/setup_linux.sh
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/setup_linux.sh
rename to dfall_ws/src/dfall_pkg/crazyradio/setup_linux.sh
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/test/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/crazyflie/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/test/crazyflie/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/crazyflie/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/crazyflie/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/crazyflie/test_swarm.py b/dfall_ws/src/dfall_pkg/crazyradio/test/crazyflie/test_swarm.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/crazyflie/test_swarm.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/crazyflie/test_swarm.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/crazyflie/test_syncCrazyflie.py b/dfall_ws/src/dfall_pkg/crazyradio/test/crazyflie/test_syncCrazyflie.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/crazyflie/test_syncCrazyflie.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/crazyflie/test_syncCrazyflie.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/crazyflie/test_syncLogger.py b/dfall_ws/src/dfall_pkg/crazyradio/test/crazyflie/test_syncLogger.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/crazyflie/test_syncLogger.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/crazyflie/test_syncLogger.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/crtp/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/test/crtp/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/crtp/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/crtp/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/crtp/test_crtpstack.py b/dfall_ws/src/dfall_pkg/crazyradio/test/crtp/test_crtpstack.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/crtp/test_crtpstack.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/crtp/test_crtpstack.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/support/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/test/support/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/support/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/support/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/support/asyncCallbackCaller.py b/dfall_ws/src/dfall_pkg/crazyradio/test/support/asyncCallbackCaller.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/support/asyncCallbackCaller.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/support/asyncCallbackCaller.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/utils/__init__.py b/dfall_ws/src/dfall_pkg/crazyradio/test/utils/__init__.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/utils/__init__.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/utils/__init__.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/test/utils/test_callbacks.py b/dfall_ws/src/dfall_pkg/crazyradio/test/utils/test_callbacks.py
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/test/utils/test_callbacks.py
rename to dfall_ws/src/dfall_pkg/crazyradio/test/utils/test_callbacks.py
diff --git a/pps_ws/src/d_fall_pps/crazyradio/tools/build/bdist b/dfall_ws/src/dfall_pkg/crazyradio/tools/build/bdist
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/tools/build/bdist
rename to dfall_ws/src/dfall_pkg/crazyradio/tools/build/bdist
diff --git a/pps_ws/src/d_fall_pps/crazyradio/tools/build/build b/dfall_ws/src/dfall_pkg/crazyradio/tools/build/build
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/tools/build/build
rename to dfall_ws/src/dfall_pkg/crazyradio/tools/build/build
diff --git a/pps_ws/src/d_fall_pps/crazyradio/tools/build/test b/dfall_ws/src/dfall_pkg/crazyradio/tools/build/test
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/tools/build/test
rename to dfall_ws/src/dfall_pkg/crazyradio/tools/build/test
diff --git a/pps_ws/src/d_fall_pps/crazyradio/tools/build/verify b/dfall_ws/src/dfall_pkg/crazyradio/tools/build/verify
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/tools/build/verify
rename to dfall_ws/src/dfall_pkg/crazyradio/tools/build/verify
diff --git a/pps_ws/src/d_fall_pps/crazyradio/tox.ini b/dfall_ws/src/dfall_pkg/crazyradio/tox.ini
similarity index 100%
rename from pps_ws/src/d_fall_pps/crazyradio/tox.ini
rename to dfall_ws/src/dfall_pkg/crazyradio/tox.ini
diff --git a/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
new file mode 100644
index 0000000000000000000000000000000000000000..f3f96349c1ddc4a27997d2d72b9264e6076447b4
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
@@ -0,0 +1,132 @@
+// Copyright (C) 2019, 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 <iostream>
+#include <string>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <ros/network.h>
+
+// Include the standard message types
+//#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+using namespace dfall_pkg;
+//using namespace std;
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+// GET YAML PARAMETER FUNCTIONS
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+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);
+
+
+
+// FUNCTIONS FOR GETTING IDs AND NAMESPACES
+
+// Get the "agentID" and "coordID" from the client node
+bool getAgentIDandCoordIDfromClientNode( std::string client_namespace , int * agentID_ref , int * coordID_ref);
+
+// Construct the namespaces for the coordinator
+void constructNamespaceForCoordinator( int coordID, std::string & coord_namespace );
+
+// Construct the namespaces for the coordinator's parameter services
+void constructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace );
+
+// Check the header of a message for whether it is relevant
+bool checkMessageHeader( int agentID , bool shouldCheckForAgentID , const std::vector<uint> & agentIDs );
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/BatteryMonitor.h b/dfall_ws/src/dfall_pkg/include/nodes/BatteryMonitor.h
new file mode 100644
index 0000000000000000000000000000000000000000..8cffa07adf79644bca45edb46039014a0cced797
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/BatteryMonitor.h
@@ -0,0 +1,204 @@
+// Copyright (C) 2019, 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 <iostream>
+#include <string>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <ros/network.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+#include "classes/GetParamtersAndNamespaces.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+using namespace dfall_pkg;
+//using namespace std;
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// Battery levels
+// #define BATTERY_LEVEL_000 0
+// #define BATTERY_LEVEL_010 1
+// #define BATTERY_LEVEL_020 2
+// #define BATTERY_LEVEL_030 3
+// #define BATTERY_LEVEL_040 4
+// #define BATTERY_LEVEL_050 5
+// #define BATTERY_LEVEL_060 6
+// #define BATTERY_LEVEL_070 7
+// #define BATTERY_LEVEL_080 8
+// #define BATTERY_LEVEL_090 9
+// #define BATTERY_LEVEL_100 10
+// #define BATTERY_LEVEL_UNAVAILABLE -1
+
+// Battery states
+#define BATTERY_STATE_NORMAL 0
+#define BATTERY_STATE_LOW 1
+
+// The types, i.e., agent or coordinator
+#define TYPE_INVALID -1
+#define TYPE_COORDINATOR 1
+#define TYPE_AGENT 2
+
+// Flying states
+#define AGENT_OPERATING_STATE_MOTORS_OFF 1
+#define AGENT_OPERATING_STATE_TAKE_OFF 2
+#define AGENT_OPERATING_STATE_FLYING 3
+#define AGENT_OPERATING_STATE_LAND 4
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+
+// The status of the crazyradio as received via messages
+//int m_crazyradio_status;
+
+// The flying state of the agent, as received via messages
+int m_agent_operating_state;
+
+// Publisher for the filtered battery voltage
+ros::Publisher filteredBatteryVoltagePublisher;
+
+// Publisher for the battery level
+ros::Publisher batteryLevelPublisher;
+
+// Publisher for changes in the battery state
+ros::Publisher batteryStateChangedPublisher;
+
+
+
+// VARIABLES THAT ARE LOADED FROM THE YAML FILE
+
+// Frequency of requesting the battery voltage, in [milliseconds]
+//float battery_polling_period = 200.0f;
+
+// Battery thresholds while in the "motors off" state, in [Volts]
+float yaml_battery_voltage_threshold_lower_while_standby = 3.30f;
+float yaml_battery_voltage_threshold_upper_while_standby = 4.20f;
+
+// Battery thresholds while in the "flying" state, in [Volts]
+float yaml_battery_voltage_threshold_lower_while_flying = 2.60f;
+float yaml_battery_voltage_threshold_upper_while_flying = 3.70f;
+
+// Delay before changing the state of the battery, in [number of measurements]
+// > Note that the delay in seconds therefore depends on the polling period
+int yaml_battery_delay_threshold_to_change_state = 5;
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// Callback for the raw voltage data
+void newBatteryVoltageCallback(const std_msgs::Float32& msg);
+// The selector for the filter
+float newBatteryVoltageForFilter(float new_value);
+// Moving average filter
+float movingAverageBatteryFilter(float new_input);
+
+
+// > For converting a voltage to a percentage,
+// depending on the current "my_flying_state" value
+float fromVoltageToPercent(float voltage , int operating_state);
+// > For converting the percentage to a level
+int convertPercentageToLevel(float percentage);
+// > For updating the battery state based on the battery level
+// Possible states are {normal,low}, and changes are delayed
+void updateBatteryStateBasedOnLevel(int level);
+
+
+// LOAD YAML PARAMETER FUNCTIONS
+void isReadyBatteryMonitorYamlCallback(const IntWithHeader & msg);
+void fetchBatteryMonitorYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CentralManagerService.h
similarity index 85%
rename from pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h
rename to dfall_ws/src/dfall_pkg/include/nodes/CentralManagerService.h
index 9d7f81e8e9323f5eb9f46c6031e7688f3e9efcdf..5079edfe6b1e35c7edcfe7b63142357d1391874d 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CentralManagerService.h
@@ -43,14 +43,23 @@
#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 the standard message types
+#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+// Include the DFALL message types
+#include "dfall_pkg/CrazyflieContext.h"
+#include "dfall_pkg/CrazyflieDB.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/CMRead.h"
+#include "dfall_pkg/CMQuery.h"
+#include "dfall_pkg/CMUpdate.h"
+#include "dfall_pkg/CMCommand.h"
+
+// Include other classes
#include "CrazyflieIO.h"
@@ -73,20 +82,7 @@
#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 dfall_pkg;
using namespace std;
@@ -103,6 +99,8 @@ using namespace std;
CrazyflieDB crazyflieDB;
+ros::Publisher m_databaseChangedPublisher;
+
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..0843e1a25220581d77dc5ad49472c6ff2b8b257a
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
@@ -0,0 +1,225 @@
+// 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:
+// Constants that are used across multiple files
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// U U
+// U U
+// U U
+// U U
+// UUU
+// ----------------------------------------------------------------------------------
+
+
+// Conversions between degrees and radians
+#define RAD2DEG 180.0/PI
+#define DEG2RAD PI/180.0
+
+// PI
+#define PI 3.141592653589
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// The types, i.e., agent or coordinator
+#define TYPE_INVALID -1
+#define TYPE_COORDINATOR 1
+#define TYPE_AGENT 2
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board the Crazyflie 2.0.
+// The following is a short description about each mode:
+//
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+//
+// CF_COMMAND_TYPE_RATE
+// 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.
+//
+// CF_COMMAND_TYPE_ANGLE
+// 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_MOTORS 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// Types of controllers being used:
+#define DEFAULT_CONTROLLER 1
+#define DEMO_CONTROLLER 2
+#define STUDENT_CONTROLLER 3
+#define MPC_CONTROLLER 4
+#define REMOTE_CONTROLLER 5
+#define TUNING_CONTROLLER 6
+#define PICKER_CONTROLLER 7
+#define TEMPLATE_CONTROLLER 8
+
+// The constants that "command" changes in the
+// operation state of this agent
+#define CMD_USE_DEFAULT_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_USE_PICKER_CONTROLLER 7
+#define CMD_USE_TEMPLATE_CONTROLLER 8
+
+
+#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
+#define STATE_UNAVAILABLE 5
+
+
+// Commands for CrazyRadio
+#define CMD_RECONNECT 0
+#define CMD_DISCONNECT 1
+
+
+// CrazyRadio states:
+#define CRAZY_RADIO_STATE_CONNECTED 0
+#define CRAZY_RADIO_STATE_CONNECTING 1
+#define CRAZY_RADIO_STATE_DISCONNECTED 2
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
+// ----------------------------------------------------------------------------------
+
+// Battery levels
+#define BATTERY_LEVEL_000 0
+#define BATTERY_LEVEL_010 1
+#define BATTERY_LEVEL_020 2
+#define BATTERY_LEVEL_030 3
+#define BATTERY_LEVEL_040 4
+#define BATTERY_LEVEL_050 5
+#define BATTERY_LEVEL_060 6
+#define BATTERY_LEVEL_070 7
+#define BATTERY_LEVEL_080 8
+#define BATTERY_LEVEL_090 9
+#define BATTERY_LEVEL_100 10
+#define BATTERY_LEVEL_UNAVAILABLE -1
+
+// Battery states
+#define BATTERY_STATE_NORMAL 0
+#define BATTERY_STATE_LOW 1
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// Y Y A M M L
+// Y Y A A MM MM L
+// Y A A M M M L
+// Y AAAAA M M L
+// Y A A M M LLLLL
+// ----------------------------------------------------------------------------------
+
+// For where to load the yaml file from
+#define LOAD_YAML_FROM_AGENT 1
+#define LOAD_YAML_FROM_COORDINATOR 2
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// For standard buttons in the GUI
+#define REQUEST_DEFAULT_SETPOINT_BUTTON_ID 100
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/CrazyflieIO.h b/dfall_ws/src/dfall_pkg/include/nodes/CrazyflieIO.h
similarity index 96%
rename from pps_ws/src/d_fall_pps/include/nodes/CrazyflieIO.h
rename to dfall_ws/src/dfall_pkg/include/nodes/CrazyflieIO.h
index fa4e25738fdea1f25235730d9e0855ef33480248..d28f56569d1a19ecbea51b385cb969d108f484f2 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/CrazyflieIO.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CrazyflieIO.h
@@ -30,9 +30,9 @@
// ----------------------------------------------------------------------------------
-#include "d_fall_pps/CrazyflieDB.h"
+#include "dfall_pkg/CrazyflieDB.h"
-namespace d_fall_pps {
+namespace dfall_pkg {
void readCrazyflieDB(CrazyflieDB& cfdb);
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
similarity index 52%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
rename to dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
index 13382717afcd986dd53802e851654e8964486949..a9beaf7ed658f18b79df4af6b7a30c9f250fed49 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
@@ -1,20 +1,20 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2019, 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
@@ -25,54 +25,32 @@
//
//
// DESCRIPTION:
-// Coordinator GUI main window.
+// Constants that are used across the Default Controler
+// Service, its GUI, and the Flying Agent Client
//
// ----------------------------------------------------------------------------------
-#include "mainwindow.h"
-#include "ui_mainwindow.h"
-MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
- QMainWindow(parent),
- ui(new Ui::MainWindow)
-{
-#ifdef CATKIN_MAKE
- //ROS_INFO("[flyingAgentGUI] Debug Point 5");
- m_rosNodeThread = new rosNodeThread(argc, argv, "flyingAgentGUI");
-#endif
-#ifdef CATKIN_MAKE
- m_rosNodeThread->init();
-#endif
- ui->setupUi(this);
- // ADD KEYBOARD SHORTCUTS
- // > For "kill GUI node", press "CTRL+C" while the GUI window is the focus
- m_close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
+// TO REQUEST MANOEUVRES
+#define DEFAULT_CONTROLLER_REQUEST_TAKEOFF 1
+#define DEFAULT_CONTROLLER_REQUEST_LANDING 2
-}
+// TO IDENITFY THE STATE OF THE DEFAULT CONTROLLER
-MainWindow::~MainWindow()
-{
- delete ui;
-}
+#define DEFAULT_CONTROLLER_STATE_NORMAL 1
+#define DEFAULT_CONTROLLER_STATE_STANDBY 99
+#define DEFAULT_CONTROLLER_STATE_UNKNOWN -1
+// > The sequence of states for a TAKE-OFF manoeuvre
+#define DEFAULT_CONTROLLER_STATE_TAKEOFF_SPIN_MOTORS 11
+#define DEFAULT_CONTROLLER_STATE_TAKEOFF_MOVE_UP 12
+#define DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT 13
+#define DEFAULT_CONTROLLER_STATE_TAKEOFF_INTEGRATOR_ON 14
-void MainWindow::on_actionShowHide_Coordinator_triggered()
-{
- //ui->customWidget_enableControllerLoadYamlBar->setEnabled(false);
- if ( ui->customWidget_coordinator->isHidden() )
- {
- ui->customWidget_coordinator->show();
- QString qstr = "Hide Coordinator";
- ui->actionShowHide_Coordinator->setText(qstr);
- }
- else
- {
- ui->customWidget_coordinator->hide();
- QString qstr = "Show Coordinator";
- ui->actionShowHide_Coordinator->setText(qstr);
- }
-}
+// > The sequence of states for a LANDING manoeuvre
+#define DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN 21
+#define DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS 22
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..b415d9875e153e1327001631918c73e44edda140
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -0,0 +1,470 @@
+// Copyright (C) 2019, 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 fall-back 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>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/IntIntService.h"
+#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+#include "nodes/DefaultControllerConstants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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.
+
+// NOTE: many constants are already defined in the "Constant.h" header file
+
+// These constants define the method used for computing
+// the control actions from the state error estimates.
+// The following is a short description about each mode:
+//
+// CONTROLLER_METHOD_RATES
+// Uses the poisition, linear velocity and angle
+// error estimates to compute the rates
+//
+// CONTROLLER_METHOD_RATE_ANGLE_NESTED
+// Uses the position and linear velocity error
+// estimates to compute an angle, and then uses
+// this as a reference to construct an angle error
+// estimate and compute from that the rates
+//
+#define CONTROLLER_METHOD_RATES 1
+#define CONTROLLER_METHOD_RATE_ANGLE_NESTED 2 // (DEFAULT)
+
+
+// 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)
+
+
+// These constants deine the behaviour of the intergrator
+#define INTEGRATOR_FLAG_ON 1
+#define INTEGRATOR_FLAG_OFF 2
+#define INTEGRATOR_FLAG_RESET 3
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 PERFORMING THE TAKE-OFF AND LANDING MANOEUVRES
+
+// The current state of the Default Controller
+int m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
+
+// A flag for when the state is changed, this is used
+// so that a "one-off" operation can be performed
+// the first time after changing that state
+bool m_current_state_changed = false;
+
+// The elapased time, incremented by counting the motion
+// capture callbacks
+float m_time_in_seconds = 0.0;
+
+// PARAMETERS FROM THE YAML FILE
+
+// Max setpoint change per second
+float yaml_max_setpoint_change_per_second_horizontal = 0.60;
+float yaml_max_setpoint_change_per_second_vertical = 0.40;
+
+// Max error for z
+float yaml_max_setpoint_error_z = 0.4;
+
+// Max error for xy
+float yaml_max_setpoint_error_xy = 0.3;
+
+// Max {roll,pitch} angle request
+float yaml_max_roll_pitch_request_degrees = 30.0;
+float yaml_max_roll_pitch_request_radians = 30.0 * DEG2RAD;
+
+// Max error for yaw angle
+float yaml_max_setpoint_error_yaw_degrees = 60.0;
+float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;
+
+// Theshold for {roll,pitch} angle beyond
+// which the motors are turned off
+float yaml_threshold_roll_pitch_for_turn_off_degrees = 70.0;
+float yaml_threshold_roll_pitch_for_turn_off_radians = 70.0 * DEG2RAD;
+
+// The thrust for take off spin motors
+float yaml_takeoff_spin_motors_thrust = 8000;
+// The time for: take off spin(-up) motors
+float yaml_takoff_spin_motors_time = 0.8;
+
+// Height change for the take off move-up
+float yaml_takeoff_move_up_start_height = 0.1;
+float yaml_takeoff_move_up_end_height = 0.4;
+// The time for: take off spin motors
+float yaml_takoff_move_up_time = 2.0;
+
+// Minimum and maximum allowed time for: take off goto setpoint
+float yaml_takoff_goto_setpoint_nearby_time = 1.0;
+float yaml_takoff_goto_setpoint_max_time = 4.0;
+
+// Box within which to keep the integrator on
+// > Units of [meters]
+// > The box consider is plus/minus this value
+float yaml_takoff_integrator_on_box_horizontal = 0.25;
+float yaml_takoff_integrator_on_box_vertical = 0.15;
+// The time for: take off integrator-on
+float yaml_takoff_integrator_on_time = 2.0;
+
+
+// Height change for the landing move-down
+float yaml_landing_move_down_end_height_setpoint = 0.05;
+float yaml_landing_move_down_end_height_threshold = 0.10;
+// The time for: landing move-down
+float yaml_landing_move_down_time_max = 5.0;
+
+// The thrust for landing spin motors
+float yaml_landing_spin_motors_thrust = 10000;
+// The time for: landing spin motors
+float yaml_landing_spin_motors_time = 1.5;
+
+
+
+
+// The setpoint to be tracked, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+float m_setpoint[4] = {0.0,0.0,0.4,0.0};
+
+// The setpoint that is actually used by the controller, this
+// differs from the setpoint when smoothing is turned on
+float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
+
+// Boolean for whether to limit rate of change of the setpoint
+bool m_shouldSmoothSetpointChanges = true;
+
+
+
+
+
+// ------------------------------------------------------
+// VARIABLES THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
+
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+
+// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
+// > the mass of the crazyflie, in [grams]
+float yaml_cf_mass_in_grams = 25.0;
+// > the weight of the Crazyflie in Newtons, i.e., mg
+float m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81 / 1000.0;
+
+// > the frequency at which the controller is running
+float yaml_control_frequency = 200.0;
+float m_control_deltaT = (1.0/200.0);
+
+// > the coefficients of the 16-bit command to thrust conversion
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+// > the min and max for saturating 16 bit thrust commands
+float yaml_command_sixteenbit_min = 1000;
+float yaml_command_sixteenbit_max = 65000;
+
+// > the default setpoint, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
+
+// Boolean indiciating whether the "Debug Message" of this
+// agent should be published or not
+bool yaml_shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM
+// should be displayed or not
+bool yaml_shouldDisplayDebugInfo = false;
+
+
+
+// VARIABLES FOR THE CONTROLLER
+
+// > A flag for which controller to use:
+int yaml_controller_method = CONTROLLER_METHOD_RATE_ANGLE_NESTED;
+
+// The LQR Controller parameters for z-height
+std::vector<float> yaml_gainMatrixThrust_2StateVector = { 0.98, 0.25};
+// The LQR Controller parameters for "CONTROLLER_METHOD_RATES"
+std::vector<float> yaml_gainMatrixRollRate_3StateVector = {-6.20,-3.00, 5.20};
+std::vector<float> yaml_gainMatrixPitchRate_3StateVector = { 6.20, 3.00, 5.20};
+// The LQR Controller parameters for "CONTROLLER_METHOD_RATE_ANGLE_NESTED"
+std::vector<float> yaml_gainMatrixRollAngle_2StateVector = {-0.20,-0.20};
+std::vector<float> yaml_gainMatrixPitchAngle_2StateVector = { 0.20, 0.20};
+float yaml_gainRollRate_fromAngle = 4.00;
+float yaml_gainPitchRate_fromAngle = 4.00;
+// The LQR Controller parameters for yaw
+float yaml_gainYawRate_fromAngle = 2.30;
+// Integrator gains
+float yaml_integratorGain_forThrust = 0.0;
+float yaml_integratorGain_forTauXY = 0.0;
+float yaml_integratorGain_forTauYaw = 0.0;
+
+
+// VARIABLES FOR THE ESTIMATOR
+
+// Frequency at which the controller is running
+float m_estimator_frequency = 200.0;
+
+// > A flag for which estimator to use:
+int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+
+// > The current state interial estimate,
+// for use by the controller
+float m_current_stateInertialEstimate[9];
+
+// > The measurement of the Crazyflie at the "current" time step,
+// to avoid confusion
+float m_current_xzy_rpy_measurement[6];
+
+// > The measurement of the Crazyflie at the "previous" time step,
+// used for computing finite difference velocities
+float m_previous_xzy_rpy_measurement[6];
+
+// > The full 12 state estimate maintained by the finite
+// difference state estimator
+float m_stateInterialEstimate_viaFiniteDifference[9];
+
+// > The full 12 state estimate maintained by the point mass
+// kalman filter state estimator
+float m_stateInterialEstimate_viaPointMassKalmanFilter[9];
+
+// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+// > For the (x,y,z) position
+std::vector<float> yaml_PMKF_Ahat_row1_for_positions = { 0.6723, 0.0034};
+std::vector<float> yaml_PMKF_Ahat_row2_for_positions = {-12.9648, 0.9352};
+std::vector<float> yaml_PMKF_Kinf_for_positions = { 0.3277,12.9648};
+
+
+// VARIABLES RELATING TO PUBLISHING
+
+// ROS Publisher for debugging variables
+ros::Publisher m_debugPublisher;
+
+// ROS Publisher to inform the network about
+// changes to the setpoint
+ros::Publisher m_setpointChangedPublisher;
+
+
+// ROS Publisher for sending motors-off command
+// to the flying agent client
+ros::Publisher m_motorsOffToFlyingAgentClientPublisher;
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// functions can be written in any order in the ".cpp" file.
+// (If the function prototypes are not included then the functions
+// need to written below in an order that ensure each function is
+// implemented before it is called from another function)
+
+// CALLBACK FOR THE REQUEST MANOEUVRE SERVICE
+bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Response &response);
+
+// CONTROLLER COMPUTATIONS
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// > For the normal state
+void computeResponse_for_normal(Controller::Response &response);
+// > For the standby state (also used for unknown state)
+void computeResponse_for_standby(Controller::Response &response);
+// > For the take-off phases
+void computeResponse_for_takeoff_move_up(Controller::Response &response);
+void computeResponse_for_takeoff_spin_motors(Controller::Response &response);
+void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response);
+void computeResponse_for_takeoff_integrator_on(Controller::Response &response);
+// > For the landing phases
+void computeResponse_for_landing_move_down(Controller::Response &response);
+void computeResponse_for_landing_spin_motors(Controller::Response &response);
+
+// SMOOTHING SETPOINT CHANGES
+void smoothSetpointChanges( float target_setpoint[4] , float (¤t_setpoint)[4] );
+
+// > This function constructs the error in the body frame
+// before calling the appropriate control function
+void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateInertial[9], Controller::Response &response, int integrator_flag);
+// > The various functions that implement an LQR controller
+void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controller::Response &response, int integrator_flag);
+
+// 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 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);
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+
+// PUBLISH THE CURRENT SETPOINT AND STATE
+void publishCurrentSetpointAndState();
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
+
+// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
+void publish_motors_off_to_flying_agent_client();
+
+// LOADING OF YAML PARAMETERS
+void timerCallback_initial_load_yaml(const ros::TimerEvent&);
+void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
+void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DemoControllerService.h
similarity index 88%
rename from pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
rename to dfall_ws/src/dfall_pkg/include/nodes/DemoControllerService.h
index 7c0f469f4a949d5e217c943d82d30ff6fae7929f..111c7e4722b7253a962047a78d08a677233de335 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DemoControllerService.h
@@ -50,18 +50,36 @@
#include "ros/ros.h"
#include <ros/package.h>
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.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 "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
+#include "dfall_pkg/CustomButton.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
-#include <std_msgs/Int32.h>
+
+// Namespacing the package
+using namespace dfall_pkg;
@@ -77,26 +95,33 @@
// 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.
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board 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
+//
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+//
+// CF_COMMAND_TYPE_RATE
+// 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.
+//
+// CF_COMMAND_TYPE_ANGLE
+// 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_MOTORS 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
@@ -125,6 +150,9 @@
#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
@@ -148,10 +176,6 @@
#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
-// Namespacing the package
-using namespace d_fall_pps;
-
-
@@ -292,9 +316,9 @@ 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;
+std::string m_namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
-std::string namespace_to_coordinator_parameter_service;
+std::string m_namespace_to_coordinator_parameter_service;
// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
@@ -320,7 +344,10 @@ bool shouldDisplayDebugInfo = false;
// POSITION
// The ID of this agent, i.e., the ID of this compute
-int my_agentID = 0;
+int m_agentID = 0;
+
+// The ID of this agent, i.e., the ID of this compute
+int m_coordID = 0;
// Boolean indicating whether the (x,y,z,yaw) of this agent should be published or not
// > The default behaviour is: do not publish,
@@ -432,14 +459,7 @@ void customCommandReceivedCallback(const CustomButton& commandReceived);
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 isReadyDemoControllerYamlCallback(const IntWithHeader & msg);
+void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
new file mode 100644
index 0000000000000000000000000000000000000000..2491524e6a8d0fe5b1b8c9dc337da1da581e74b8
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -0,0 +1,363 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+// 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 the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/CrazyflieContext.h"
+#include "dfall_pkg/Setpoint.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/CMQuery.h"
+#include "dfall_pkg/IntIntService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+#include "nodes/DefaultControllerConstants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+// VARIABLES FOR THE MOTION CAPTURE DATA
+// > The index for which element in the "motion capture
+// data" array is expected to match the name in
+// "m_context", (negative numbers indicate unknown)
+int m_poseDataIndex = -1;
+// > Boolen indicating if the Mocap data is availble
+bool m_isAvailable_mocap_data = false;
+// > Boolen indicating if the object is "long term" occuled
+bool m_isOcculded_mocap_data = false;
+// > Number of consecutive occulsions that trigger the
+// "m_isOcculded_mocap_data" variable to be "true"
+int yaml_consecutive_occulsions_threshold = 30;
+// > Timer that when triggered determines that the
+// "m_isAvailable_mocap_data" variable becomes true
+ros::Timer m_timer_mocap_timeout_check;
+// > Time out duration after which Mocap is considered unavailable
+float yaml_mocap_timeout_duration = 1.0;
+
+
+
+// VARIABLES FOR STORING THE PARAMTER OF THE POSITION
+// AND TILT ANGLE SAFTY CHECKS
+// > Boolean indicating whether the tilt angle check
+// should be performed
+bool yaml_isEnabled_strictSafety = true;
+// > The maximum allowed tilt angle, in degrees and radians
+float yaml_maxTiltAngle_for_strictSafety_degrees = 70;
+float yaml_maxTiltAngle_for_strictSafety_radians = 70 * DEG2RAD;
+
+
+
+// VARIABLES FOR MANAGING THE FLYING STATE
+// > Integer that is the current flying state
+int m_flying_state;
+// > Booleans for whether the {take-off,landing} should
+// be performed with the default controller
+bool yaml_shouldPerfom_takeOff_with_defaultController = true;
+bool yaml_shouldPerfom_landing_with_defaultController = true;
+// > Service Clients for requesting the Default controller
+// to perform a {take-off,landing} maneouvre
+ros::ServiceClient m_defaultController_requestManoeuvre;
+// > Timer that fire when the {take-off,landing} is complete
+ros::Timer m_timer_takeoff_complete;
+ros::Timer m_timer_land_complete;
+
+
+
+// VARIABLES RELATING TO CONTROLLER SELECTION
+// > Integer indicating the controller that is to be
+// used in when motion capture data is received
+int m_instant_controller;
+// > Pointer to the controller service client that
+// agrees with the "m_instant_controller" variable
+ros::ServiceClient* m_instant_controller_service_client;
+// > Boolean indicating that the controller service clients
+// have been successfully created
+bool m_controllers_avialble = false;
+// > Timer for creating the controller service client after
+// some delay
+ros::Timer m_timer_for_createAllControllerServiceClients;
+// > Integer indicating the controller that has been
+// requested. This controller is used during the "flying"
+// state, and the "Default" controller is used during the
+// "take-off" and "landing" states.
+int m_controller_nominally_selected;
+
+
+
+// VARIABLES FOR THE CONTROLER SERVIVCE CLIENTS
+// The default controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_defaultController;
+// The Student controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_studentController;
+// The Tuning controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_tuningController;
+// The Picker controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_pickerController;
+// The Template controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_templateController;
+
+
+
+
+
+
+
+// variable for crazyradio status
+int m_crazyradio_status;
+
+//describes the area of the crazyflie and other parameters
+CrazyflieContext m_context;
+
+// Service Client from which the "CrazyflieContext" is loaded
+ros::ServiceClient m_centralManager;
+
+// Publisher for the control actions to be sent on
+// to the Crazyflie (the CrazyRadio node listen to this
+// publisher and actually send the commands)
+// {onboardControllerType,roll,pitch,yaw,motorCmd1,motorCmd2,motorCmd3,motorCmd4}
+ros::Publisher m_commandForSendingToCrazyfliePublisher;
+
+// Publisher for the current flying state of this Flying Agent Client
+ros::Publisher m_flyingStatePublisher;
+
+// Publisher for the commands of:
+// {take-off,land,motors-off, and which controller to use}
+//ros::Publisher commandPublisher;
+
+// Publisher Communication with crazyRadio node. Connect and disconnect
+ros::Publisher m_crazyRadioCommandPublisher;
+
+// Publisher for which controller is currently being used
+ros::Publisher m_controllerUsedPublisher;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+// FUNCTIONS FOR HANDLING THE MOTION CAPTURE DATA
+// > Callback run every time new motion capture
+// data is available
+void viconCallback(const ViconData& viconData);
+// > For extracting the pose data of an specific
+// object by name
+int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index , CrazyflieData& pose);
+// > For converting the global frame motion capture
+// data to the local frame of this agent
+void coordinatesToLocal(CrazyflieData& cf);
+// > Callback run when motion capture data has not
+// been receive for a specified time
+void timerCallback_mocap_timeout_check(const ros::TimerEvent&);
+// > For sending a command, via the CrazyRadio, that
+// the motors should be set to zero
+void sendZeroOutputCommandForMotors();
+
+
+
+// FOR THE SAFETY CHECKS ON POSITION AND TILT ANGLE
+bool safetyCheck_on_positionAndTilt(CrazyflieData data);
+
+
+
+// FUNCTIONS THAT MANAGE THE CHANGES TO THE FLYING STATE
+// > For changing between possible state of:
+// {take-off,flying,take-off,motors-off}
+void requestChangeFlyingStateTo(int new_state);
+// > For changing to take-off
+void requestChangeFlyingStateToTakeOff();
+// > For changing to land
+void requestChangeFlyingStateToLand();
+// > Callback that the take off phase is complete
+void takeOffTimerCallback(const ros::TimerEvent&);
+// > Callback that the landing phase is complete
+void landTimerCallback(const ros::TimerEvent&);
+
+
+
+// FUNCTIONS FOR SELECTING WHICH CONTROLLER TO USE
+// > For setting the controller that is actually used
+void setInstantController(int controller);
+// > For retrieving the value of the class variable
+int getInstantController();
+// > For setting the controller that it to be used
+// during the normal "flying" state
+void setControllerNominallySelected(int controller);
+// > For retrieving the value of the class variable
+int getControllerNominallySelected();
+
+
+
+// THE CALLBACK THAT A NEW FLYING STATE WAS REQUESTED
+// > These requests come from the "Flying Agent GUI"
+// > The options are: {take-off,land,motor-off,controller}
+void flyingStateOrControllerRequestCallback(const IntWithHeader & commandMsg);
+
+
+
+// THE CALLBACK THAT THE CRAZY RADIO STATUS CHANGED
+void crazyRadioStatusCallback(const std_msgs::Int32& msg);
+
+
+
+// THE CALLBACK THAT AN EMERGENCY STOP MESSAGE WAS RECEIVED
+void emergencyStopReceivedCallback(const IntWithHeader & msg);
+
+
+
+// THE SERVICE CALLBACK REQUESTING THE CURRENT FLYING STATE
+bool getCurrentFlyingStateServiceCallback(IntIntService::Request &request, IntIntService::Response &response);
+
+
+
+// FOR THE BATTERY STATE CALLBACK
+void batteryMonitorStateChangedCallback(std_msgs::Int32 msg);
+
+
+
+// FUNCTIONS FOR THE CONTEXT OF THIS AGENT
+// > Callback that the context database changed
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
+// > For loading the context of this agent
+void loadCrazyflieContext();
+
+
+
+// FUNCTIONS FOR CREATING THE CONTROLLER SERVICE CLIENT
+// > For creating a service client from the name
+// of the YAML parameter
+void createControllerServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient );
+// > For creating an IntInt service client from the
+// name of a YAML paramter
+void createIntIntServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient );
+// > Callback for the timer so that all services servers
+// exists before we try to create the clients
+void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&);
+
+
+
+// FOR LOADING THE YAML PARAMETERS
+void isReadyFlyingAgentClientConfigYamlCallback(const IntWithHeader & msg);
+void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h b/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h
similarity index 80%
rename from pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
rename to dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h
index 36153c11a6fe4639fd150f7c588c4db3769d16fd..d298e9e2b2456acf03f5aac57ed73ba69d341ee5 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
@@ -47,15 +47,22 @@
#include <ros/ros.h>
#include <ros/package.h>
#include <ros/network.h>
+
+// Include the standard message types
#include "std_msgs/Int32.h"
//#include "std_msgs/Float32.h"
-//#include <std_msgs/String.h>
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/FloatWithHeader.h"
+#include "dfall_pkg/StringWithHeader.h"
-#include "d_fall_pps/Controller.h"
-#include "d_fall_pps/LoadYamlFiles.h"
+// Include the DFALL service types
+#include "dfall_pkg/LoadYamlFromFilename.h"
// Include the shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
// ----------------------------------------------------------------------------------
@@ -67,13 +74,13 @@
// ----------------------------------------------------------------------------------
// The types, i.e., agent or coordinator
-#define TYPE_INVALID -1
-#define TYPE_COORDINATOR 1
-#define TYPE_AGENT 2
+//#define TYPE_INVALID -1
+//#define TYPE_COORDINATOR 1
+//#define TYPE_AGENT 2
// Namespacing the package
-using namespace d_fall_pps;
+using namespace dfall_pkg;
//using namespace std;
@@ -89,20 +96,20 @@ using namespace d_fall_pps;
// The type of this node, i.e., agent or a coordinator, specified as a parameter in the
// "*.launch" file
-int my_type = 0;
+int m_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";
+// The ID of this node
+int m_ID = 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 controllerYamlReadyForFetchPublisher;
+// The namespace into which this parameter service loads yaml parameters
+std::string m_base_namespace;
+
+// Publisher for passing a service request onto the
+// loadinging function
+ros::Publisher requestLoadYamlFilenamePublisher;
-std::string m_base_namespace;
-ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
// ----------------------------------------------------------------------------------
@@ -119,6 +126,10 @@ ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
// P R R OOO T OOO T Y P EEEEE SSSS
// ----------------------------------------------------------------------------------
-void requestLoadControllerYamlCallback(const std_msgs::Int32& msg);
+bool requestLoadYamlFilenameCallbackServiceCallback(LoadYamlFromFilename::Request &request, LoadYamlFromFilename::Response &response);
+
+void requestLoadYamlFilenameCallback(const StringWithHeader& yamlFilenameToLoad);
+
+bool getTypeAndIDParameters();
-bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &response);
+bool constructNamespaces();
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerConstants.h
similarity index 63%
rename from pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h
rename to dfall_ws/src/dfall_pkg/include/nodes/PickerControllerConstants.h
index 124eda803cbf57011e73e6b2c4308fa854b2414f..05e62b808e37e98c5733807d1cb7fa23267a353f 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/mainwindow.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerConstants.h
@@ -1,20 +1,20 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2019, 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
@@ -25,49 +25,32 @@
//
//
// DESCRIPTION:
-// Coordinator GUI main window header.
+// Constants that are used across the Picker Controler Service and GUI
//
// ----------------------------------------------------------------------------------
-#ifndef MAINWINDOW_FLYINGAGENTGUI_H
-#define MAINWINDOW_FLYINGAGENTGUI_H
-
-#include <QMainWindow>
-#include <QShortcut>
-
-#ifdef CATKIN_MAKE
-#include <ros/ros.h>
-#include <ros/network.h>
-#include <ros/package.h>
-#include "rosNodeThread_for_flyingAgentGUI.h"
-#endif
-
-
-namespace Ui {
-class MainWindow;
-}
-class MainWindow : public QMainWindow
-{
- Q_OBJECT
-public:
- explicit MainWindow(int argc, char **argv, QWidget *parent = 0);
- ~MainWindow();
-private slots:
- void on_actionShowHide_Coordinator_triggered();
+// TO IDENITFY THE STATE OF THE PICKER
-private:
- Ui::MainWindow *ui;
+#define PICKER_STATE_UNKNOWN -1
+#define PICKER_STATE_STANDBY 99
+#define PICKER_STATE_GOTO_START 1
+#define PICKER_STATE_ATTACH 2
+#define PICKER_STATE_LIFT_UP 3
+#define PICKER_STATE_GOTO_END 4
+#define PICKER_STATE_PUT_DOWN 5
+#define PICKER_STATE_SQUAT 6
+#define PICKER_STATE_JUMP 7
- QShortcut* m_close_GUI_shortcut;
-#ifdef CATKIN_MAKE
- rosNodeThread* m_rosNodeThread;
-#endif
-};
+// DEFAULT {x,y,z,yaw,mass}
-#endif // MAINWINDOW_H
+#define PICKER_DEFAULT_X 0
+#define PICKER_DEFAULT_Y 0
+#define PICKER_DEFAULT_Z 0.4
+#define PICKER_DEFAULT_YAW_DEGREES 0
+#define PICKER_DEFAULT_MASS_GRAMS 30
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..37528947f52b3f399bbad44ba047e89070d536cd
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
@@ -0,0 +1,383 @@
+// Copyright (C) 2019, 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>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+//#include "dfall_pkg/CustomButtonWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+//#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+#include "nodes/PickerControllerConstants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Needed for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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.
+
+// 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
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 PICKER SERVICE
+
+// The current state of the picker, i.e.,
+// {goto start, attach, lift up, goto end, put down,
+// squat, jump, standby}
+int m_picker_current_state = PICKER_STATE_STANDBY;
+
+// Current time
+//int m_time_ticks = 0;
+//float m_time_seconds;
+
+// > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
+float m_mass_total_in_grams;
+float m_weight_total_in_newtons;
+
+// The setpoint to be tracked, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+float m_setpoint[4] = {0.0,0.0,0.4,0.0};
+
+// The setpoint that is actually used by the controller, this
+// differs from the setpoint when smoothing is turned on
+float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
+
+// Boolean for whether to limit rate of change of the setpoint
+bool m_shouldSmoothSetpointChanges = true;
+
+// Max setpoint change per second
+float yaml_max_setpoint_change_per_second_horizontal = 0.1;
+float yaml_max_setpoint_change_per_second_vertical = 0.1;
+float yaml_max_setpoint_change_per_second_yaw_degrees = 90.0;
+float m_max_setpoint_change_per_second_yaw_radians = 90.0 * DEG2RAD;
+
+
+
+
+
+// ------------------------------------------------------
+// VARIABLES THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
+
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+// VARIABLES FOR THE CONTOLLER
+
+// > the mass of the crazyflie, in [grams]
+float yaml_mass_cf_in_grams = 30.0;
+
+// > the coefficients of the 16-bit command to thrust conversion
+//std::vector<float> yaml_motorPoly(3);
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+// > the frequency at which the controller is running
+float yaml_control_frequency = 200.0;
+
+// > the default setpoint, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
+
+// The weight of the Crazyflie in Newtons, i.e., mg
+float m_weight_cf_in_newtons = 0.0;
+
+// The location error of the Crazyflie at the "previous" time step
+std::vector<float> m_previous_stateErrorInertial = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+std::vector<float> yaml_gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixRollRate = { 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixPitchRate = { 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00};
+std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
+
+
+// The 16-bit command limits
+float yaml_cmd_sixteenbit_min = 1000;
+float yaml_cmd_sixteenbit_max = 60000;
+
+
+
+// VARIABLES FOR THE ESTIMATOR
+
+// Frequency at which the controller is running
+float m_estimator_frequency = 200.0;
+
+// > A flag for which estimator to use:
+int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+// > The current state interial estimate,
+// for use by the controller
+float m_current_stateInertialEstimate[12];
+
+// > The measurement of the Crazyflie at the "current" time step,
+// to avoid confusion
+float m_current_xzy_rpy_measurement[6];
+
+// > The measurement of the Crazyflie at the "previous" time step,
+// used for computing finite difference velocities
+float m_previous_xzy_rpy_measurement[6];
+
+// > The full 12 state estimate maintained by the finite
+// difference state estimator
+float m_stateInterialEstimate_viaFiniteDifference[12];
+
+// > The full 12 state estimate maintained by the point mass
+// kalman filter state estimator
+float m_stateInterialEstimate_viaPointMassKalmanFilter[12];
+
+// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+// > For the (x,y,z) position
+std::vector<float> yaml_PMKF_Ahat_row1_for_positions = { 0.6723, 0.0034};
+std::vector<float> yaml_PMKF_Ahat_row2_for_positions = {-12.9648, 0.9352};
+std::vector<float> yaml_PMKF_Kinf_for_positions = { 0.3277,12.9648};
+// > For the (roll,pitch,yaw) angles
+std::vector<float> yaml_PMKF_Ahat_row1_for_angles = { 0.6954, 0.0035};
+std::vector<float> yaml_PMKF_Ahat_row2_for_angles = {-11.0342, 0.9448};
+std::vector<float> yaml_PMKF_Kinf_for_angles = { 0.3046,11.0342};
+
+
+
+// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
+
+// Boolean whether to execute the convert into body frame function
+bool yaml_shouldPerformConvertIntoBodyFrame = true;
+
+
+// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool yaml_shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool yaml_shouldDisplayDebugInfo = false;
+
+// ROS Publisher for debugging variables
+ros::Publisher m_debugPublisher;
+
+
+// VARIABLES RELATING TO COMMUNICATING THE SETPOINT
+
+// ROS Publisher for inform the network about
+// changes to the setpoint
+ros::Publisher m_setpointChangedPublisher;
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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.
+
+
+// ADDED FOR THE PICKER
+void perControlCycleOperations();
+
+
+// SEPARATE CALLBACK FUNCTIONS FOR EACH BUTTON
+void buttonPressed_goto_start();
+void buttonPressed_attach();
+void buttonPressed_lift_up();
+void buttonPressed_goto_end();
+void buttonPressed_put_down();
+void buttonPressed_squat();
+void buttonPressed_jump();
+void buttonPressed_standby();
+
+
+
+
+// CONTROLLER COMPUTATIONS
+// > The function that is called to "start" all estimation and control computations
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// > The function that smooth changes in the setpoin
+void smoothSetpointChanges();
+
+// > The various functions that implement an LQR controller
+void calculateControlOutput_viaLQRforRates(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);
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE SETPOINT FUNCTION
+//void setNewSetpoint(float x, float y, float z, float yaw);
+void setNewSetpoint(int state, bool should_smooth, float x, float y, float z, float yaw, float mass);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+
+// 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);
+
+// FOR LOADING THE YAML PARAMETERS
+void isReadyPickerControllerYamlCallback(const IntWithHeader & msg);
+void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
similarity index 98%
rename from pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
rename to dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
index a8adb30557c94b51091ee23ece23a8bfba345ead..968f78ec66df2c2b79f45ac7d4a8d86749aeab37 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
@@ -56,16 +56,16 @@
#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 "dfall_pkg/ViconData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
+#include "dfall_pkg/CustomButton.h"
+#include "dfall_pkg/ViconSubscribeObjectName.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
#include <std_msgs/Int32.h>
@@ -82,9 +82,6 @@
// 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.
@@ -166,7 +163,7 @@
#define LQR_ANGLE_MODE 2
// Namespacing the package
-using namespace d_fall_pps;
+using namespace dfall_pkg;
// ----------------------------------------------------------------------------------
diff --git a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h
similarity index 79%
rename from pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
rename to dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h
index 19a1a90d349978f5b4c2392b9a5350400dae4305..3a540f297544dfa279d053e34d8265c3e606ba61 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h
@@ -48,18 +48,34 @@
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <ros/package.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
+//#include <std_msgs/String.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 the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
-#include <std_msgs/Int32.h>
// Include the shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
+
+
@@ -73,29 +89,36 @@
// 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.
+// NOTE: these constants are already defined in the "Constant.h" header file
+// and are repeated here for convenience
+
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board 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
+//
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+//
+// CF_COMMAND_TYPE_RATE
+// 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.
+//
+// CF_COMMAND_TYPE_ANGLE
+// 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_MOTORS 6
+//#define CF_COMMAND_TYPE_RATE 7
+//#define CF_COMMAND_TYPE_ANGLE 8
+
-// Namespacing the package
-using namespace d_fall_pps;
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..91d6b3e89d67aec767d7c5cf8fe3a740a16a8497
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h
@@ -0,0 +1,257 @@
+// Copyright (C) 2019, 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>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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.
+
+
+
+// NOTE: these constants are already defined in the "Constant.h" header file
+// and are repeated here for convenience
+
+// These constants define the modes that can be used for controller this is
+// running on-board the Crazyflie 2.0.
+// Therefore, the constants defined here need to be in agreement with those
+// defined in the firmware running on-board the Crazyflie 2.0.
+// The following is a short description about each mode:
+//
+// CF_COMMAND_TYPE_MOTORS
+// In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+//
+// CF_COMMAND_TYPE_RATE
+// 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.
+//
+// CF_COMMAND_TYPE_ANGLE
+// 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_MOTORS 6
+//#define CF_COMMAND_TYPE_RATE 7
+//#define CF_COMMAND_TYPE_ANGLE 8
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
+// > the mass of the crazyflie, in [grams]
+float yaml_cf_mass_in_grams = 25.0;
+
+// The weight of the Crazyflie in Newtons, i.e., mg
+float m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81 / 1000.0;
+
+// > the coefficients of the 16-bit command to thrust conversion
+//std::vector<float> yaml_motorPoly(3);
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+// > the frequency at which the controller is running
+float yaml_control_frequency = 200.0;
+
+// > the default setpoint, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
+
+
+
+
+
+// The location error of the Crazyflie at the "previous" time step
+float m_previous_stateErrorInertial[9];
+
+// The setpoint to be tracked, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
+
+
+// The LQR Controller parameters for "LQR_RATE_MODE"
+std::vector<float> m_gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
+std::vector<float> m_gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
+std::vector<float> m_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
+std::vector<float> m_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 m_debugPublisher;
+
+// ROS Publisher for inform the network about
+// changes to the setpoin
+ros::Publisher m_setpointChangedPublisher;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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);
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
+
+// FOR LOADING THE YAML PARAMETERS
+void timerCallback_initial_load_yaml(const ros::TimerEvent&);
+void isReadyStudentControllerYamlCallback(const IntWithHeader & msg);
+void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/TemplateControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/TemplateControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..d7e69289335f6a9c92aefd85bcc6879b47712980
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TemplateControllerService.h
@@ -0,0 +1,237 @@
+// Copyright (C) 2019, 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:
+// A Template Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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.
+
+// NOTE: many constants are already defined in the
+// "Constant.h" header file
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
+// > the mass of the crazyflie, in [grams]
+float yaml_cf_mass_in_grams = 25.0;
+
+// > the frequency at which the controller is running
+float yaml_control_frequency = 200.0;
+
+// > the coefficients of the 16-bit command to thrust conversion
+//std::vector<float> yaml_motorPoly(3);
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+
+// The min and max for saturating 16 bit thrust commands
+float yaml_command_sixteenbit_min = 1000;
+float yaml_command_sixteenbit_max = 60000;
+
+// > the default setpoint, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool yaml_shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool yaml_shouldDisplayDebugInfo = false;
+
+// The LQR Controller parameters for "LQR_RATE_MODE"
+std::vector<float> yaml_gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixRollRate = { 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixPitchRate = { 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00};
+std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
+
+
+
+// The weight of the Crazyflie in Newtons, i.e., mg
+float m_cf_weight_in_newtons = 0.0;
+
+// The location error of the Crazyflie at the "previous" time step
+float m_previous_stateErrorInertial[9];
+
+// The setpoint to be tracked, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
+
+
+
+
+// ROS Publisher for debugging variables
+ros::Publisher m_debugPublisher;
+
+// ROS Publisher for inform the network about
+// changes to the setpoin
+ros::Publisher m_setpointChangedPublisher;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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);
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
+
+// FOR LOADING THE YAML PARAMETERS
+void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg);
+void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
similarity index 85%
rename from pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
rename to dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
index 9866bdbe93c3a79cbc59c8470aaf75ea08b8cd8b..d6eed8366f359e6f998fcc8ab668a562f72c4edb 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
@@ -55,19 +55,49 @@
#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 the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
-// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/FloatWithHeader.h"
+//#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+//#include "dfall_pkg/CustomButtonWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+//#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
-#include <std_msgs/Int32.h>
+// Include the DFALL service types
+#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+
+
+
+// //the generated structs from the msg-files have to be included
+// #include "dfall_pkg/ViconData.h"
+// #include "dfall_pkg/Setpoint.h"
+// #include "dfall_pkg/ControlCommand.h"
+// #include "dfall_pkg/Controller.h"
+// #include "dfall_pkg/DebugMsg.h"
+// #include "dfall_pkg/CustomButton.h"
+// #include "dfall_pkg/ViconSubscribeObjectName.h"
+
+// // Include the Parameter Service shared definitions
+// #include "nodes/Constants.h"
+
+// #include <std_msgs/Int32.h>
@@ -82,9 +112,6 @@
// 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.
@@ -166,7 +193,7 @@
#define LQR_ANGLE_MODE 2
// Namespacing the package
-using namespace d_fall_pps;
+using namespace dfall_pkg;
// ----------------------------------------------------------------------------------
@@ -178,6 +205,18 @@ using namespace d_fall_pps;
// ----------------------------------------------------------------------------------
+
+
+float m_gain_P = 0.31;
+float m_gain_P_to_D_ratio = 0.6;
+
+
+
+
+
+
+
+
// ******************************************************************************* //
// VARIABLES SPECIFIC TO THE TUNING CONTROL FEATURE
@@ -262,7 +301,7 @@ 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;
+int controller_mode = CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED;
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
@@ -361,13 +400,13 @@ 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;
+std::string m_namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
-std::string namespace_to_coordinator_parameter_service;
+std::string m_namespace_to_coordinator_parameter_service;
// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
-ros::Publisher debugPublisher;
+ros::Publisher m_debugPublisher;
// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
@@ -388,8 +427,11 @@ 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;
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
@@ -464,24 +506,45 @@ void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float
// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
float computeMotorPolyBackward(float thrust);
+
+
+
// SETPOINT CHANGE CALLBACK
-void setpointCallback(const Setpoint& newSetpoint);
+//void setpointCallback(const Setpoint& newSetpoint);
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+//bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestGainChangeCallback(const FloatWithHeader& newGain);
// 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();
+// 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();
+
+void isReadyTuningControllerYamlCallback(const IntWithHeader & msg);
+void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle);
// ******************************************************************************* //
diff --git a/dfall_ws/src/dfall_pkg/launch/Config.sh b/dfall_ws/src/dfall_pkg/launch/Config.sh
new file mode 100755
index 0000000000000000000000000000000000000000..028d739f3f049ffe9266b537789a09a4ff504bb5
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/launch/Config.sh
@@ -0,0 +1,9 @@
+# TO RUN THE SYSTEM FULLY ON THE LOCAL COMPUTER:
+#export ROS_MASTER_URI=http://localhost:11311
+# TO RUN THE SYSTEM ON A DEFAULT CONFIGURATION OF THE NETWROK:
+export ROS_MASTER_URI=http://teacher:11311
+# OTHER NECESSARY ENVIRONMENT VARIABLES:
+export ROS_IP=$(hostname -I | awk '{print $1;}')
+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'
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/dfall_ws/src/dfall_pkg/launch/agent.launch
similarity index 55%
rename from pps_ws/src/d_fall_pps/launch/Agent.launch
rename to dfall_ws/src/dfall_pkg/launch/agent.launch
index a24b9ad1c03a1e998038f6978487b545dae47259..52a40b9441559c9293b0cf457e0ce058906a2990 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/dfall_ws/src/dfall_pkg/launch/agent.launch
@@ -3,6 +3,9 @@
<!-- INPUT ARGUMENT OF THE AGENT's ID -->
<arg name="agentID" default="$(optenv DFALL_DEFAULT_AGENT_ID)" />
+ <!-- INPUT ARGUMENT OF THE COORDINATOR's ID -->
+ <arg name="coordID" default="$(optenv DFALL_DEFAULT_COORD_ID)" />
+
<!-- INPUT ARGUMENT FOR LAUNCHING A GUI OR NOT -->
<arg name="withGUI" default="true" />
@@ -10,34 +13,52 @@
<!-- <param name="param" value="$(arg agentID)"/> -->
<!-- Example of how to specify the agentID from command line -->
- <!-- roslaunch d_fall_pps agentID:=1 -->
+ <!-- roslaunch dfall_pkg agentID:=1 -->
<group ns="$(eval 'agent' + str(agentID).zfill(3))">
<!-- CRAZY RADIO -->
<node
- pkg = "d_fall_pps"
+ pkg = "dfall_pkg"
name = "CrazyRadio"
output = "screen"
type = "CrazyRadio.py"
>
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
+ <rosparam command="load" file="$(find dfall_pkg)/param/BatteryMonitor.yaml" />
</node>
- <!-- PPS CLIENT -->
+ <!-- FLYING AGENT CLIENT -->
<node
- pkg = "d_fall_pps"
- name = "PPSClient"
+ pkg = "dfall_pkg"
+ name = "FlyingAgentClient"
output = "screen"
- type = "PPSClient"
+ type = "FlyingAgentClient"
>
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
<param name="agentID" value="$(arg agentID)" />
+ <param name="coordID" value="$(arg coordID)" />
+ </node>
+
+ <!-- BATTERY MONITOR -->
+ <node
+ pkg = "dfall_pkg"
+ name = "BatteryMonitor"
+ output = "screen"
+ type = "BatteryMonitor"
+ >
+ </node>
+
+ <!-- DEFAULT CONTROLLER -->
+ <node
+ pkg = "dfall_pkg"
+ name = "DefaultControllerService"
+ output = "screen"
+ type = "DefaultControllerService"
+ >
</node>
<!-- SAFE CONTROLLER -->
<node
- pkg = "d_fall_pps"
+ pkg = "dfall_pkg"
name = "SafeControllerService"
output = "screen"
type = "SafeControllerService"
@@ -46,7 +67,7 @@
<!-- DEMO CONTROLLER -->
<node
- pkg = "d_fall_pps"
+ pkg = "dfall_pkg"
name = "DemoControllerService"
output = "screen"
type = "DemoControllerService"
@@ -55,25 +76,16 @@
<!-- STUDENT CONTROLLER -->
<node
- pkg = "d_fall_pps"
+ pkg = "dfall_pkg"
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"
+ pkg = "dfall_pkg"
name = "RemoteControllerService"
output = "screen"
type = "RemoteControllerService"
@@ -82,7 +94,7 @@
<!-- TUNING CONTROLLER -->
<node
- pkg = "d_fall_pps"
+ pkg = "dfall_pkg"
name = "TuningControllerService"
output = "screen"
type = "TuningControllerService"
@@ -91,72 +103,79 @@
<!-- PICKER CONTROLLER -->
<node
- pkg = "d_fall_pps"
+ pkg = "dfall_pkg"
name = "PickerControllerService"
output = "screen"
type = "PickerControllerService"
>
</node>
+ <!-- TEMPLATE CONTROLLER -->
+ <node
+ pkg = "dfall_pkg"
+ name = "TemplateControllerService"
+ output = "screen"
+ type = "TemplateControllerService"
+ >
+ </node>
+
<!-- PARAMETER SERVICE -->
<node
- pkg = "d_fall_pps"
+ pkg = "dfall_pkg"
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"
- />
+ <param name="agentID" value="$(arg agentID)" />
<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"
+ file = "$(find dfall_pkg)/param/FlyingAgentClientConfig.yaml"
+ ns = "FlyingAgentClientConfig"
/>
<rosparam
command = "load"
- file = "$(find d_fall_pps)/param/StudentController.yaml"
- ns = "StudentController"
+ file = "$(find dfall_pkg)/param/BatteryMonitor.yaml"
+ ns = "BatteryMonitor"
/>
<rosparam
command = "load"
- file = "$(find d_fall_pps)/param/MpcController.yaml"
- ns = "MpcController"
+ file = "$(find dfall_pkg)/param/SafeController.yaml"
+ ns = "SafeController"
/>
<rosparam
command = "load"
- file = "$(find d_fall_pps)/param/RemoteController.yaml"
+ file = "$(find dfall_pkg)/param/RemoteController.yaml"
ns = "RemoteController"
/>
<rosparam
command = "load"
- file = "$(find d_fall_pps)/param/TuningController.yaml"
+ file = "$(find dfall_pkg)/param/TuningController.yaml"
ns = "TuningController"
/>
<rosparam
command = "load"
- file = "$(find d_fall_pps)/param/PickerController.yaml"
+ file = "$(find dfall_pkg)/param/PickerController.yaml"
ns = "PickerController"
/>
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/TemplateController.yaml"
+ ns = "TemplateController"
+ />
</node>
<!-- AGENT GUI (aka. the "student GUI") -->
<group if="$(arg withGUI)">
<node
- pkg = "d_fall_pps"
- name = "student_GUI"
+ pkg = "dfall_pkg"
+ name = "FlyingAgentGUI"
output = "screen"
- type = "student_GUI">
+ type = "FlyingAgentGUI"
+ >
+ <param name="type" type="str" value="agent" />
+ <param name="agentID" value="$(arg agentID)" />
</node>
</group>
diff --git a/dfall_ws/src/dfall_pkg/launch/coordinator.launch b/dfall_ws/src/dfall_pkg/launch/coordinator.launch
new file mode 100755
index 0000000000000000000000000000000000000000..c9e7e15414acb8efb1d0361afd4768eb2f8e2ae1
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/launch/coordinator.launch
@@ -0,0 +1,54 @@
+<launch>
+
+ <!-- INPUT ARGUMENT OF THE COORDINATOR's ID -->
+ <arg name="coordID" default="$(optenv DFALL_DEFAULT_COORD_ID)" />
+
+ <!-- INPUT ARGUMENT FOR LAUNCHING A GUI OR NOT -->
+ <arg name="withGUI" default="true" />
+
+ <!-- Example of how to use the value in coordID -->
+ <!-- <param name="param" value="$(arg coordID)"/> -->
+
+ <!-- Example of how to specify the coordID from command line -->
+ <!-- roslaunch dfall_pkg coordID:=001 -->
+
+ <group ns="coord$(arg coordID)">
+
+ <!-- COORDINATOR GUI -->
+ <group if="$(arg withGUI)">
+ <node
+ pkg="dfall_pkg"
+ name="FlyingAgentGUI"
+ output="screen"
+ type="FlyingAgentGUI"
+ >
+ <param name="type" type="str" value="coordinator" />
+ <param name="coordID" value="$(arg coordID)" />
+ </node>
+ </group>
+
+
+ <!-- PARAMETER SERVICE -->
+ <node
+ pkg = "dfall_pkg"
+ name = "ParameterService"
+ output = "screen"
+ type = "ParameterService"
+ >
+ <param name="type" type="str" value="coordinator" />
+ <param name="coordID" value="$(arg coordID)" />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/BatteryMonitor.yaml"
+ ns = "YamlFileNames"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/FlyingAgentClientConfig.yaml"
+ ns = "SafeController"
+ />
+ </node>
+
+ </group>
+
+</launch>
diff --git a/dfall_ws/src/dfall_pkg/launch/teacher.launch b/dfall_ws/src/dfall_pkg/launch/teacher.launch
new file mode 100755
index 0000000000000000000000000000000000000000..a9235436a8c4ae791fb1dda34e5dca37585bc498
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/launch/teacher.launch
@@ -0,0 +1,31 @@
+<launch>
+
+ <!-- CENTRAL MANAGER -->
+ <node
+ pkg="dfall_pkg"
+ name="CentralManagerService"
+ output="screen"
+ type="CentralManagerService"
+ >
+ </node>
+
+ <!-- VICON DATA PUBLISHER -->
+ <node
+ pkg="dfall_pkg"
+ name="ViconDataPublisher"
+ output="screen"
+ type="ViconDataPublisher"
+ >
+ <rosparam command="load" file="$(find dfall_pkg)/param/ViconConfig.yaml" />
+ </node>
+
+ <!-- TEACHER GUI -->
+ <node
+ pkg="dfall_pkg"
+ name="SystemConfigGUI"
+ output="screen"
+ type="SystemConfigGUI"
+ >
+ </node>
+
+</launch>
diff --git a/pps_ws/src/d_fall_pps/msg/AreaBounds.msg b/dfall_ws/src/dfall_pkg/msg/AreaBounds.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/AreaBounds.msg
rename to dfall_ws/src/dfall_pkg/msg/AreaBounds.msg
diff --git a/pps_ws/src/d_fall_pps/msg/ControlCommand.msg b/dfall_ws/src/dfall_pkg/msg/ControlCommand.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/ControlCommand.msg
rename to dfall_ws/src/dfall_pkg/msg/ControlCommand.msg
diff --git a/pps_ws/src/d_fall_pps/msg/CrazyflieContext.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieContext.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/CrazyflieContext.msg
rename to dfall_ws/src/dfall_pkg/msg/CrazyflieContext.msg
diff --git a/pps_ws/src/d_fall_pps/msg/CrazyflieDB.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieDB.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/CrazyflieDB.msg
rename to dfall_ws/src/dfall_pkg/msg/CrazyflieDB.msg
diff --git a/pps_ws/src/d_fall_pps/msg/CrazyflieData.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieData.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/CrazyflieData.msg
rename to dfall_ws/src/dfall_pkg/msg/CrazyflieData.msg
diff --git a/pps_ws/src/d_fall_pps/msg/CrazyflieEntry.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieEntry.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/CrazyflieEntry.msg
rename to dfall_ws/src/dfall_pkg/msg/CrazyflieEntry.msg
diff --git a/pps_ws/src/d_fall_pps/msg/CustomButton.msg b/dfall_ws/src/dfall_pkg/msg/CustomButton.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/CustomButton.msg
rename to dfall_ws/src/dfall_pkg/msg/CustomButton.msg
diff --git a/dfall_ws/src/dfall_pkg/msg/CustomButtonWithHeader.msg b/dfall_ws/src/dfall_pkg/msg/CustomButtonWithHeader.msg
new file mode 100644
index 0000000000000000000000000000000000000000..87fbf0fcad9d96d0d591b9912a4f33633c49b757
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/msg/CustomButtonWithHeader.msg
@@ -0,0 +1,7 @@
+uint32 button_index
+bool bool_data
+int32 int_data
+float64 float_data
+string string_data
+bool shouldCheckForAgentID
+uint32[] agentIDs
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/msg/DebugMsg.msg b/dfall_ws/src/dfall_pkg/msg/DebugMsg.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/DebugMsg.msg
rename to dfall_ws/src/dfall_pkg/msg/DebugMsg.msg
diff --git a/dfall_ws/src/dfall_pkg/msg/FloatWithHeader.msg b/dfall_ws/src/dfall_pkg/msg/FloatWithHeader.msg
new file mode 100755
index 0000000000000000000000000000000000000000..229050a8a26f64e22a91eb2a1bd87074e265ff97
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/msg/FloatWithHeader.msg
@@ -0,0 +1,3 @@
+float32 data
+bool shouldCheckForAgentID
+uint32[] agentIDs
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/IntWithHeader.msg b/dfall_ws/src/dfall_pkg/msg/IntWithHeader.msg
new file mode 100755
index 0000000000000000000000000000000000000000..1eb4c40ca3c831ed1e18fddf6e09ce65a5937203
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/msg/IntWithHeader.msg
@@ -0,0 +1,3 @@
+uint32 data
+bool shouldCheckForAgentID
+uint32[] agentIDs
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/msg/Setpoint.msg b/dfall_ws/src/dfall_pkg/msg/Setpoint.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/Setpoint.msg
rename to dfall_ws/src/dfall_pkg/msg/Setpoint.msg
diff --git a/pps_ws/src/d_fall_pps/msg/SetpointV2.msg b/dfall_ws/src/dfall_pkg/msg/SetpointV2.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/SetpointV2.msg
rename to dfall_ws/src/dfall_pkg/msg/SetpointV2.msg
diff --git a/dfall_ws/src/dfall_pkg/msg/SetpointWithHeader.msg b/dfall_ws/src/dfall_pkg/msg/SetpointWithHeader.msg
new file mode 100644
index 0000000000000000000000000000000000000000..cdf1562a19264e182d534bd33d4fc12a2413c5fe
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/msg/SetpointWithHeader.msg
@@ -0,0 +1,9 @@
+float64 x
+float64 y
+float64 z
+float64 yaw
+float64 mass
+bool isChecked
+uint32 buttonID
+bool shouldCheckForAgentID
+uint32[] agentIDs
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/StringWithHeader.msg b/dfall_ws/src/dfall_pkg/msg/StringWithHeader.msg
new file mode 100755
index 0000000000000000000000000000000000000000..3c03b7a25061d9e24135cee6da806ee438ef128e
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/msg/StringWithHeader.msg
@@ -0,0 +1,3 @@
+string data
+bool shouldCheckForAgentID
+uint32[] agentIDs
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/msg/UnlabeledMarker.msg b/dfall_ws/src/dfall_pkg/msg/UnlabeledMarker.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/UnlabeledMarker.msg
rename to dfall_ws/src/dfall_pkg/msg/UnlabeledMarker.msg
diff --git a/pps_ws/src/d_fall_pps/msg/ViconData.msg b/dfall_ws/src/dfall_pkg/msg/ViconData.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/ViconData.msg
rename to dfall_ws/src/dfall_pkg/msg/ViconData.msg
diff --git a/pps_ws/src/d_fall_pps/msg/ViconSubscribeObjectName.msg b/dfall_ws/src/dfall_pkg/msg/ViconSubscribeObjectName.msg
similarity index 100%
rename from pps_ws/src/d_fall_pps/msg/ViconSubscribeObjectName.msg
rename to dfall_ws/src/dfall_pkg/msg/ViconSubscribeObjectName.msg
diff --git a/pps_ws/src/d_fall_pps/package.xml b/dfall_ws/src/dfall_pkg/package.xml
similarity index 87%
rename from pps_ws/src/d_fall_pps/package.xml
rename to dfall_ws/src/dfall_pkg/package.xml
index 91b8fcda82c7bcdc5e137eaa13d327e13211aca9..f07c112ed29a21871110b192995fc681f4c08ebe 100755
--- a/pps_ws/src/d_fall_pps/package.xml
+++ b/dfall_ws/src/dfall_pkg/package.xml
@@ -1,25 +1,25 @@
<?xml version="1.0"?>
<package>
- <name>d_fall_pps</name>
+ <name>dfall_pkg</name>
<version>0.0.0</version>
- <description>The d_fall_pps package</description>
+ <description>The Distributed Flying and Localisation Laboratory (D-FaLL) package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
- <maintainer email="crazyflie@todo.todo">crazyflie</maintainer>
+ <maintainer email="pbeuchat43@gmail.com">crazyflie</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
- <license>TODO</license>
+ <license>GPLv3</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/d_fall_pps</url> -->
+ <!-- <url type="website">http://wiki.ros.org/dfall_pkg</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
diff --git a/dfall_ws/src/dfall_pkg/param/BatteryMonitor.yaml b/dfall_ws/src/dfall_pkg/param/BatteryMonitor.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..3aeb59762f27449e478446117c073a1361425224
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/BatteryMonitor.yaml
@@ -0,0 +1,14 @@
+# Frequency of requesting the battery voltage, in [milliseconds]
+battery_polling_period: 200
+
+# Battery thresholds while in the "motors off" state, in [Volts]
+battery_voltage_threshold_lower_while_standby: 3.30
+battery_voltage_threshold_upper_while_standby: 4.20
+
+# Battery thresholds while in the "flying" state, in [Volts]
+battery_voltage_threshold_lower_while_flying: 2.60
+battery_voltage_threshold_upper_while_flying: 3.70
+
+# Delay before changing the state of the battery, in [number of measurements]
+# > Note that the delay in seconds therefore depends on the polling period
+battery_delay_threshold_to_change_state: 5
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..64d77b13533f071c327fe852406f07169db9ad59
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
@@ -0,0 +1,129 @@
+# ------------------------------------------------------
+# PARAMTERS FOR THE TAKE-OFF AND LANDING MANOEUVRES
+
+# Max setpoint change per second
+max_setpoint_change_per_second_horizontal : 0.60 # [meters]
+max_setpoint_change_per_second_vertical : 0.40 # [meters]
+
+# Max error for z
+max_setpoint_error_z: 0.4
+
+# Max error for xy
+max_setpoint_error_xy: 0.3
+
+# Max {roll,pitch} angle request
+max_roll_pitch_request_degrees: 30
+
+# Max error for yaw angle
+max_setpoint_error_yaw_degrees: 60
+
+# Theshold for {roll,pitch} angle beyond
+# which the motors are turned off
+threshold_roll_pitch_for_turn_off_degrees: 70
+
+# The thrust for take off spin motors
+takeoff_spin_motors_thrust: 8000
+# The time for: take off spin motors
+takoff_spin_motors_time: 0.8
+
+# Height change for the take off move-up
+takeoff_move_up_start_height: 0.1
+takeoff_move_up_end_height: 0.4
+# The time for: take off move-up
+takoff_move_up_time: 2.0
+
+# Minimum and maximum allowed time for: take off goto setpoint
+takoff_goto_setpoint_nearby_time: 1.0
+takoff_goto_setpoint_max_time: 4.0
+
+# Box within which to keep the integrator on
+# > Units of [meters]
+# > The box consider is plus/minus this value
+takoff_integrator_on_box_horizontal: 0.25
+takoff_integrator_on_box_vertical: 0.15
+# The time for: take off integrator-on
+takoff_integrator_on_time: 2.0
+
+
+# Height change for the landing move-down
+landing_move_down_end_height_setpoint: 0.05
+landing_move_down_end_height_threshold: 0.10
+# The time for: landing move-down
+landing_move_down_time_max: 5.0
+
+# The thrust for landing spin motors
+landing_spin_motors_thrust: 10000
+# The time for: landing spin motors
+landing_spin_motors_time: 1.5
+
+
+# IMPORTANT NOTE: the times above should NOT be set
+# to zero because this will cause a divide by zero
+# crash.
+
+
+# ------------------------------------------------------
+# PARAMTERS THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
+
+# The mass of the crazyflie, in [grams]
+mass : 30
+
+# 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]
+
+# The min and max for saturating 16 bit thrust commands
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 60000
+
+# The default setpoint, the ordering is (x,y,z,yaw),
+# with unit [meters,meters,meters,radians]
+default_setpoint : [0.0, 0.0, 0.4, 0.0]
+
+# 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 to use, defined as:
+# 1 - Rate controller
+# 2 - Angle-Rate nested controller
+controller_method : 2
+
+# The LQR Controller parameters for z-height
+gainMatrixThrust_2StateVector : [ 0.98, 0.25]
+# The LQR Controller parameters for mode 1 (the Rate controller)
+gainMatrixRollRate_3StateVector : [-6.20,-3.00, 5.20]
+gainMatrixPitchRate_3StateVector : [ 6.20, 3.00, 5.20]
+# The LQR Controller parameters for mode 2 (Angle-nested)
+gainMatrixRollAngle_2StateVector : [-0.20,-0.20]
+gainMatrixPitchAngle_2StateVector : [ 0.20, 0.20]
+gainRollRate_fromAngle : 4.00
+gainPitchRate_fromAngle : 4.00
+# The LQR Controller parameters for yaw
+gainYawRate_fromAngle : 2.30
+# Integrator gains
+integratorGain_forThrust : 0.00
+integratorGain_forTauXY : 0.00
+integratorGain_forTauYaw : 0.00
+
+
+
+# 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)
+estimator_method : 1
+
+# 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]
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/param/DemoController.yaml b/dfall_ws/src/dfall_pkg/param/DemoController.yaml
similarity index 100%
rename from pps_ws/src/d_fall_pps/param/DemoController.yaml
rename to dfall_ws/src/dfall_pkg/param/DemoController.yaml
diff --git a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..76f0d0b0724ea9425792bb53cf73671bdc932385
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
@@ -0,0 +1,31 @@
+# The name the controller services advertised by
+# each of the controller nodes.
+defaultController: "DefaultControllerService/DefaultController"
+studentController: "StudentControllerService/StudentController"
+tuningController: "TuningControllerService/TuningController"
+pickerController: "PickerControllerService/PickerController"
+templateController: "TemplateControllerService/TemplateController"
+
+# The name of the service advertised by the Default
+# Controller for requests manoeuvres to be performed
+defaultController_requestManoeuvre: "DefaultControllerService/RequestManoeuvre"
+
+# Flag for whether to use angle for switching
+# to the Default Controller
+isEnabled_strictSafety: true
+maxTiltAngle_for_strictSafety_degrees: 70
+
+# Number of consecutive occulsions that will deem the
+# object as "long-term" occuled
+consecutive_occulsions_threshold: 30
+
+# Time out duration after which Mocap is considered unavailable
+mocap_timeout_duration: 2.0
+
+# Flag for whether the take-off should be performed
+# with the default controller
+shouldPerfom_takeOff_with_defaultController: true
+
+# Flag for whether the landing should be performed
+# with the default controller
+shouldPerfom_landing_with_defaultController: true
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/param/MpcController.yaml b/dfall_ws/src/dfall_pkg/param/MpcController.yaml
similarity index 100%
rename from pps_ws/src/d_fall_pps/param/MpcController.yaml
rename to dfall_ws/src/dfall_pkg/param/MpcController.yaml
diff --git a/pps_ws/src/d_fall_pps/param/PickerController.yaml b/dfall_ws/src/dfall_pkg/param/PickerController.yaml
similarity index 72%
rename from pps_ws/src/d_fall_pps/param/PickerController.yaml
rename to dfall_ws/src/dfall_pkg/param/PickerController.yaml
index 13776ed98046cb69ecf15286e1dab3cbbe4e17fe..eb097bb6343e756bfd884e1dc70590f5fc108108 100644
--- a/pps_ws/src/d_fall_pps/param/PickerController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/PickerController.yaml
@@ -1,35 +1,8 @@
-# Mass of the crazyflie
-mass_CF : 32
-# Mass of the letters
-mass_E : 3.2
-mass_T : 2.9
-mass_H : 3.3
-
-# Thickness of the object at pick-up and put-down, in [meters]
-# > This should also account for extra height due to
-# the surface where the object is
-thickness_of_object_at_pickup : 0.02
-thickness_of_object_at_putdown : 0.03
-
-# (x,y) coordinates of the pickup location
-pickup_coordinates_xy : [-0.26, 0.12]
-
-# (x,y) coordinates of the drop off location
-dropoff_coordinates_xy_for_E : [ 0.33, -0.03]
-dropoff_coordinates_xy_for_T : [ 0.24, 0.00]
-dropoff_coordinates_xy_for_H : [ 0.28, 0.00]
-
-# Length of the string from the Crazyflie
-# to the end of the Picker, in [meters]
-picker_string_length : 0.30
# Max setpoint change per second
max_setpoint_change_per_second_horizontal : 0.20 # [meters]
max_setpoint_change_per_second_vertical : 0.10 # [meters]
-max_setpoint_change_per_second_yaw_degrees : 90.00 # [degrees]
-
-# Frequency of the controller, in hertz
-vicon_frequency : 200
+max_setpoint_change_per_second_yaw_degrees : 180.00 # [degrees]
@@ -39,11 +12,11 @@ vicon_frequency : 200
+# ------------------------------------------------------
+# PARAMTERS THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
-
-
-# THE FOLLOWING PARAMETERS ARE USED
-# FOR THE LOW-LEVEL CONTROLLER
+# Mass of the crazyflie
+mass_cf_in_grams : 32
# Frequency of the controller, in hertz
control_frequency : 200
@@ -55,10 +28,10 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
shouldPerformConvertIntoBodyFrame : true
# Boolean indiciating whether the (x,y,z,yaw) of this agent should be published or not
-shouldPublishCurrent_xyz_yaw : true
+#shouldPublishCurrent_xyz_yaw : true
# Boolean indiciating whether the "Debug Message" of this agent should be published or not
-shouldPublishDebugMessage : true
+shouldPublishDebugMessage : false
# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
shouldDisplayDebugInfo : false
diff --git a/pps_ws/src/d_fall_pps/param/RemoteController.yaml b/dfall_ws/src/dfall_pkg/param/RemoteController.yaml
similarity index 100%
rename from pps_ws/src/d_fall_pps/param/RemoteController.yaml
rename to dfall_ws/src/dfall_pkg/param/RemoteController.yaml
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/dfall_ws/src/dfall_pkg/param/SafeController.yaml
similarity index 100%
rename from pps_ws/src/d_fall_pps/param/SafeController.yaml
rename to dfall_ws/src/dfall_pkg/param/SafeController.yaml
diff --git a/dfall_ws/src/dfall_pkg/param/StudentController.yaml b/dfall_ws/src/dfall_pkg/param/StudentController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..737fea73b48980c4701f4c8f15ec4a9f74d3e076
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/StudentController.yaml
@@ -0,0 +1,12 @@
+# Mass of the crazyflie
+mass : 28
+
+# 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]
+
+# The default setpoint, the ordering is (x,y,z,yaw),
+# with unit [meters,meters,meters,radians]
+default_setpoint : [0.0, 0.0, 0.4, 0.0]
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/param/TemplateController.yaml b/dfall_ws/src/dfall_pkg/param/TemplateController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b7bbda03c3b2d4e8a4183b1289ce48d08cf68b3f
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/TemplateController.yaml
@@ -0,0 +1,28 @@
+# Mass of the crazyflie
+mass : 28
+
+# 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]
+
+# The min and max for saturating 16 bit thrust commands
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 60000
+
+# The default setpoint, the ordering is (x,y,z,yaw),
+# with unit [meters,meters,meters,radians]
+default_setpoint : [0.0, 0.0, 0.4, 0.0]
+
+# 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
+
+# The LQR Controller parameters for rate mode
+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]
diff --git a/pps_ws/src/d_fall_pps/param/TuningController.yaml b/dfall_ws/src/dfall_pkg/param/TuningController.yaml
similarity index 97%
rename from pps_ws/src/d_fall_pps/param/TuningController.yaml
rename to dfall_ws/src/dfall_pkg/param/TuningController.yaml
index 0bf1cb89869c7d2462bfd4db9f5d5162f1905a2d..0baaf04eafb2bea1663a35de72c8f1ea7d084fa9 100644
--- a/pps_ws/src/d_fall_pps/param/TuningController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/TuningController.yaml
@@ -40,7 +40,7 @@ multiplier_heading_max : 3.0
# Mass of the crazyflie
-mass : 29
+mass : 30
# Frequency of the controller, in hertz
vicon_frequency : 200
@@ -76,7 +76,7 @@ shouldDisplayDebugInfo : false
# - 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
+controller_mode : 5
# A flag for which estimator to use, defined as:
@@ -93,7 +93,7 @@ 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]
+gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.76, 0.00, 0.00, 0.32, 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]
diff --git a/pps_ws/src/d_fall_pps/param/ViconConfig.yaml b/dfall_ws/src/dfall_pkg/param/ViconConfig.yaml
similarity index 100%
rename from pps_ws/src/d_fall_pps/param/ViconConfig.yaml
rename to dfall_ws/src/dfall_pkg/param/ViconConfig.yaml
diff --git a/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b61a3e84ba4f77f9e3e914e2d3741baeb4212d21
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
@@ -0,0 +1,4 @@
+dictionary : {
+ 'FlyingAgentClientConfig' : 'FlyingAgentClientConfig' ,
+ 'SafeController' : 'SafeController'
+}
diff --git a/pps_ws/src/d_fall_pps/scripts/land_crazyflie b/dfall_ws/src/dfall_pkg/scripts/land_crazyflie
similarity index 50%
rename from pps_ws/src/d_fall_pps/scripts/land_crazyflie
rename to dfall_ws/src/dfall_pkg/scripts/land_crazyflie
index 38d16c7d0eab1ef9e63e1c43e50f901ab72063cc..d475b5643bbf41a53c6114f59da4d830d4449e2b 100755
--- a/pps_ws/src/d_fall_pps/scripts/land_crazyflie
+++ b/dfall_ws/src/dfall_pkg/scripts/land_crazyflie
@@ -2,5 +2,5 @@
if [ "$#" -ne 0 ]
then echo "usage: land crazyfly <no arguments>"
-else rostopic pub -1 /$ROS_NAMESPACE/PPSClient/Command std_msgs/Int32 4;
+else rostopic pub -1 /$ROS_NAMESPACE/FlyingAgentClient/Command std_msgs/Int32 4;
fi
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/scripts/load_custom_controller b/dfall_ws/src/dfall_pkg/scripts/load_custom_controller
similarity index 53%
rename from pps_ws/src/d_fall_pps/scripts/load_custom_controller
rename to dfall_ws/src/dfall_pkg/scripts/load_custom_controller
index 50a3eefddf95c32ae543bf98a90e47c58006bd7e..fb99d4e7df0948b71887ee58c42b961624a4bf99 100755
--- a/pps_ws/src/d_fall_pps/scripts/load_custom_controller
+++ b/dfall_ws/src/dfall_pkg/scripts/load_custom_controller
@@ -2,6 +2,6 @@
if [ "$#" -ne 0 ]
then echo "usage: load_custom_controller <no arguments>"
-else rostopic pub -1 /$ROS_NAMESPACE/PPSClient/Command std_msgs/Int32 2;
+else rostopic pub -1 /$ROS_NAMESPACE/FlyingAgentClient/Command std_msgs/Int32 2;
fi
diff --git a/pps_ws/src/d_fall_pps/scripts/load_safe_controller b/dfall_ws/src/dfall_pkg/scripts/load_safe_controller
similarity index 52%
rename from pps_ws/src/d_fall_pps/scripts/load_safe_controller
rename to dfall_ws/src/dfall_pkg/scripts/load_safe_controller
index 25570c5186f344e5f8a2e31ecd4d2863c6f0a744..c58ec1d37c6048cc0a264c817d1da6af83706a83 100755
--- a/pps_ws/src/d_fall_pps/scripts/load_safe_controller
+++ b/dfall_ws/src/dfall_pkg/scripts/load_safe_controller
@@ -2,6 +2,6 @@
if [ "$#" -ne 0 ]
then echo "usage: load_safe_controller <no arguments>"
-else rostopic pub -1 /$ROS_NAMESPACE/PPSClient/Command std_msgs/Int32 1;
+else rostopic pub -1 /$ROS_NAMESPACE/FlyingAgentClient/Command std_msgs/Int32 1;
fi
diff --git a/pps_ws/src/d_fall_pps/scripts/motors_off_crazyflie b/dfall_ws/src/dfall_pkg/scripts/motors_off_crazyflie
similarity index 52%
rename from pps_ws/src/d_fall_pps/scripts/motors_off_crazyflie
rename to dfall_ws/src/dfall_pkg/scripts/motors_off_crazyflie
index c4acfeefadbbf235beeefa685031c48a74d9aba8..a607a00a029bfdbf863268f7f870b398083167b9 100755
--- a/pps_ws/src/d_fall_pps/scripts/motors_off_crazyflie
+++ b/dfall_ws/src/dfall_pkg/scripts/motors_off_crazyflie
@@ -2,6 +2,6 @@
if [ "$#" -ne 0 ]
then echo "usage: motors_off_crazyflie <no arguments>"
-else rostopic pub -1 /$ROS_NAMESPACE/PPSClient/Command std_msgs/Int32 5;
+else rostopic pub -1 /$ROS_NAMESPACE/FlyingAgentClient/Command std_msgs/Int32 5;
fi
diff --git a/pps_ws/src/d_fall_pps/scripts/safe_controller_setpoint b/dfall_ws/src/dfall_pkg/scripts/safe_controller_setpoint
similarity index 73%
rename from pps_ws/src/d_fall_pps/scripts/safe_controller_setpoint
rename to dfall_ws/src/dfall_pkg/scripts/safe_controller_setpoint
index 73bb4fa77bba122f07379972d4d2410752a93df1..0ec71316960dde2f32bc3be5cd9b53b2ce57f148 100755
--- a/pps_ws/src/d_fall_pps/scripts/safe_controller_setpoint
+++ b/dfall_ws/src/dfall_pkg/scripts/safe_controller_setpoint
@@ -2,6 +2,6 @@
if [ "$#" -ne 4 ]
then echo "usage: safe_controller_setpoint <x> <y> <z> <yaw>"
-else rostopic pub -1 /$ROS_NAMESPACE/SafeControllerService/Setpoint d_fall_pps/Setpoint "{x: $1, y: $2, z: $3, yaw: $4}";
+else rostopic pub -1 /$ROS_NAMESPACE/SafeControllerService/Setpoint dfall_pkg/Setpoint "{x: $1, y: $2, z: $3, yaw: $4}";
fi
diff --git a/pps_ws/src/d_fall_pps/scripts/take_off_crazyflie b/dfall_ws/src/dfall_pkg/scripts/take_off_crazyflie
similarity index 51%
rename from pps_ws/src/d_fall_pps/scripts/take_off_crazyflie
rename to dfall_ws/src/dfall_pkg/scripts/take_off_crazyflie
index 0bc0d30655d5d58f344758e33495cc7f53eb4399..6491218d5e0c6b21bd6a119d04f029eaec2f38f0 100755
--- a/pps_ws/src/d_fall_pps/scripts/take_off_crazyflie
+++ b/dfall_ws/src/dfall_pkg/scripts/take_off_crazyflie
@@ -2,6 +2,6 @@
if [ "$#" -ne 0 ]
then echo "usage: take_off crazyfly <no arguments>"
-else rostopic pub -1 /$ROS_NAMESPACE/PPSClient/Command std_msgs/Int32 3;
+else rostopic pub -1 /$ROS_NAMESPACE/FlyingAgentClient/Command std_msgs/Int32 3;
fi
diff --git a/pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp b/dfall_ws/src/dfall_pkg/src/CrazyflieIO.cpp
similarity index 96%
rename from pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp
rename to dfall_ws/src/dfall_pkg/src/CrazyflieIO.cpp
index e5e0c03f350623f89d39b19b36556a296ef81ec3..0f8625211fa2df7d2106313f89595d34c04a6c4b 100644
--- a/pps_ws/src/d_fall_pps/src/CrazyflieIO.cpp
+++ b/dfall_ws/src/dfall_pkg/src/CrazyflieIO.cpp
@@ -44,13 +44,13 @@
#include <fstream>
#include <string>
-#include "d_fall_pps/CrazyflieContext.h"
-#include "d_fall_pps/CrazyflieEntry.h"
-#include "d_fall_pps/CrazyflieDB.h"
+#include "dfall_pkg/CrazyflieContext.h"
+#include "dfall_pkg/CrazyflieEntry.h"
+#include "dfall_pkg/CrazyflieDB.h"
using namespace std;
-namespace d_fall_pps {
+namespace dfall_pkg {
string escape(string input) {
string escaped;
@@ -116,7 +116,7 @@ vector<string> nextLine(istream& str) {
}
string getCrazyflieDBPath() {
- string packagePath = ros::package::getPath("d_fall_pps") + "/";
+ string packagePath = ros::package::getPath("dfall_pkg") + "/";
string dbFile = packagePath + "param/Crazyflie.db";
return dbFile;
}
diff --git a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d48a2214683291495c54ce62bfa20188d130f44
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
@@ -0,0 +1,234 @@
+// Copyright (C) 2019, 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:
+// A class for having the standard functions in one place
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "classes/GetParamtersAndNamespaces.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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 ( )
+// ----------------------------------------------------------------------------------
+
+
+
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+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");
+ }
+}
+
+
+
+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");
+ }
+}
+
+
+
+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();
+}
+
+
+
+
+bool getAgentIDandCoordIDfromClientNode( std::string client_namespace , int * agentID_ref , int * coordID_ref)
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Create a node handle to the client
+ ros::NodeHandle client_nodeHandle(client_namespace);
+
+ // Get the value of the "agentID" parameter
+ int agentID_fetched;
+ if(!client_nodeHandle.getParam("agentID", agentID_fetched))
+ {
+ return_was_successful = false;
+ }
+ else
+ {
+ *agentID_ref = agentID_fetched;
+ }
+
+ // Get the value of the "coordID" parameter
+ int coordID_fetched;
+ if(!client_nodeHandle.getParam("coordID", coordID_fetched))
+ {
+ return_was_successful = false;
+ }
+ else
+ {
+ *coordID_ref = coordID_fetched;
+ }
+
+ // Return
+ return return_was_successful;
+}
+
+
+
+
+
+void constructNamespaceForCoordinator( int coordID, std::string & coord_namespace )
+{
+ // Convert the ID to a zero padded string
+ std::ostringstream str_stream;
+ str_stream << std::setw(3) << std::setfill('0') << coordID;
+ std::string coordID_as_string(str_stream.str());
+ // Construct the namespace
+ coord_namespace = "/dfall/coord" + coordID_as_string;
+}
+
+
+void constructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace )
+{
+ // Convert the ID to a zero padded string
+ std::ostringstream str_stream;
+ str_stream << std::setw(3) << std::setfill('0') << coordID;
+ std::string coordID_as_string(str_stream.str());
+ // Construct the namespace
+ coord_param_service_namespace = "/dfall/coord" + coordID_as_string + "/ParameterService";
+}
+
+
+
+
+
+// Check the header of a message for whether it is relevant
+bool checkMessageHeader( int agentID , bool shouldCheckForAgentID , const std::vector<uint> & agentIDs )
+{
+ // The messag is by default relevant if the "shouldCheckForAgentID"
+ // flag is false
+ if (!shouldCheckForAgentID)
+ {
+ return true;
+ }
+ else
+ {
+ // Iterate through the vector of IDs
+ for ( int i_ID=0 ; i_ID < agentIDs.size() ; i_ID++ )
+ {
+ if ( agentIDs[i_ID] == agentID )
+ {
+ return true;
+ }
+ }
+ }
+ // If the function made it to here, then the message is
+ // NOT relevant, hence return false
+ return false;
+}
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..80ddc1daaf98098898cd7e538f82cb46e50215ab
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
@@ -0,0 +1,539 @@
+// Copyright (C) 2019, 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
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/BatteryMonitor.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
+// ----------------------------------------------------------------------------------
+
+
+
+void agentOperatingStateCallback(const std_msgs::Int32& msg)
+{
+ // Extract the data
+ m_agent_operating_state = msg.data;
+}
+
+
+
+
+
+void crazyRadioStatusCallback(const std_msgs::Int32& msg)
+{
+ // Extract the data
+ int crazyradio_status = msg.data;
+
+ // I the Crazyradio DISCONNECTED
+ if (crazyradio_status == CRAZY_RADIO_STATE_DISCONNECTED)
+ {
+ // Then publish that the battery level is unavailable
+ std_msgs::Int32 battery_level_msg;
+ battery_level_msg.data = BATTERY_LEVEL_UNAVAILABLE;
+ batteryLevelPublisher.publish(battery_level_msg);
+ }
+}
+
+
+
+
+
+void newBatteryVoltageCallback(const std_msgs::Float32& msg)
+{
+ // Extract the data
+ float battery_voltage = msg.data;
+
+ // Provide the new measurement to the filter
+ float filtered_battery_voltage = newBatteryVoltageForFilter(battery_voltage);
+
+ // Convert the filtered voltage to a percentage
+ // > Note that this depending on the operating state
+ float filtered_battery_voltage_as_percentage = fromVoltageToPercent(filtered_battery_voltage,m_agent_operating_state);
+
+ // Convert the battery percentage to a level
+ int filtered_battery_voltage_as_level = convertPercentageToLevel(filtered_battery_voltage_as_percentage);
+
+ // Publish that the battery voltage
+ std_msgs::Float32 filtered_battery_voltage_msg;
+ filtered_battery_voltage_msg.data = filtered_battery_voltage;
+ filteredBatteryVoltagePublisher.publish(filtered_battery_voltage_msg);
+
+ // Publish that the battery level
+ std_msgs::Int32 battery_level_msg;
+ battery_level_msg.data = filtered_battery_voltage_as_level;
+ batteryLevelPublisher.publish(battery_level_msg);
+
+ // Update the battery state using the level
+ // > Note that the function called sends a message
+ // only if the battery state changes
+ updateBatteryStateBasedOnLevel(filtered_battery_voltage_as_level);
+}
+
+
+
+
+// > For filtering the battery voltage
+float newBatteryVoltageForFilter(float new_value)
+{
+ return movingAverageBatteryFilter( new_value);
+}
+
+float movingAverageBatteryFilter(float new_input)
+{
+ const int N = 7;
+ static float previous_output = 4.2f;
+ static float inputs[N] = {4.2f,4.2f,4.2f,4.2f,4.2f,4.2f,4.2f};
+
+
+ // imagine an array of an even number of samples, we will output the one
+ // in the middle averaged with information from all of them.
+ // For that, we only need to store some past of the signal
+ float output = previous_output + new_input/N - inputs[N-1]/N;
+
+ // update array of inputs
+ for(int i = N - 1; i > 0; i--)
+ {
+ inputs[i] = inputs[i-1];
+ }
+ inputs[0] = new_input;
+
+
+ // update previous output
+ previous_output = output;
+ return output;
+}
+
+
+
+
+
+// > For converting a voltage to a percentage, depending on the current "my_flying_state" value
+float fromVoltageToPercent(float voltage , int operating_state )
+{
+ // INITIALISE THE LOCAL VARIABLE FOR THE VOLTAGE WHEN FULL/EMPTY
+ float voltage_when_full;
+ float voltage_when_empty;
+
+ // COMPUTE THE PERCENTAGE DIFFERENTLY DEPENDING ON
+ // THE CURRENT FLYING STATE
+ if (operating_state == AGENT_OPERATING_STATE_MOTORS_OFF)
+ {
+ // Voltage limits for a "standby" type of state
+ voltage_when_empty = yaml_battery_voltage_threshold_lower_while_standby;
+ voltage_when_full = yaml_battery_voltage_threshold_upper_while_standby;
+ }
+ else
+ {
+ // Voltage limits for a "flying" type of state
+ voltage_when_empty = yaml_battery_voltage_threshold_lower_while_flying;
+ voltage_when_full = yaml_battery_voltage_threshold_upper_while_flying;
+ }
+
+ // COMPUTE THE PERCENTAGE
+ float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
+
+ // CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
+ // > This should not happen to often
+ if(percentage > 100.0f)
+ {
+ percentage = 100.0f;
+ }
+ if(percentage < 0.0f)
+ {
+ percentage = 0.0f;
+ }
+
+ // RETURN THE PERCENTAGE
+ return percentage;
+}
+
+
+
+
+// > For converting the percentage to a level
+int convertPercentageToLevel(float percentage)
+{
+ // Initialise the battery level
+ static int battery_level = BATTERY_LEVEL_UNAVAILABLE;
+
+ // Iterate through the levels
+ for (int i_level=0 ; i_level<10 ; i_level++)
+ {
+ // Compute the threshold for this level
+ float threshold = float(i_level) * 10.0f;
+ // Add a buffer to the threshold to prevent
+ // high frequency changes to the level
+ if (battery_level==i_level)
+ {
+ threshold += 2.0f;
+ }
+ // Return the current index if appropriate
+ if (percentage <= threshold)
+ {
+ return i_level;
+ }
+ }
+ // If the function made it to this point without
+ // returning, then the percentage is at or above
+ // the maximum level
+ return BATTERY_LEVEL_100;
+}
+
+
+
+
+// > For updating the battery state based on the battery level
+// Possible states are {normal,low}, and changes are delayed
+void updateBatteryStateBasedOnLevel(int level)
+{
+ // Initialise the battery state
+ static int battery_state = BATTERY_STATE_NORMAL;
+
+ // Initialise a counter for delaying a change of state
+ static int num_since_change = 0;
+
+ if (level == 0)
+ {
+ if (battery_state == BATTERY_STATE_NORMAL)
+ {
+ num_since_change++;
+ }
+ else
+ {
+ num_since_change = 0;
+ }
+ }
+ else if (level >= 1)
+ {
+ if (battery_state == BATTERY_STATE_LOW)
+ {
+ num_since_change++;
+ }
+ else
+ {
+ num_since_change = 0;
+ }
+ }
+
+ // Check if the "delay-to-change" threshold is reached
+ if (num_since_change >= yaml_battery_delay_threshold_to_change_state)
+ {
+ if (battery_state == BATTERY_STATE_NORMAL)
+ {
+ battery_state = BATTERY_STATE_LOW;
+ }
+ else
+ {
+ battery_state = BATTERY_STATE_NORMAL;
+ }
+ // Publish the change
+ std_msgs::Int32 battery_state_msg;
+ battery_state_msg.data = battery_state;
+ batteryStateChangedPublisher.publish(battery_state_msg);
+ }
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+void isReadyBatteryMonitorYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[BATTERY MONITOR] Now fetching the BatteryMonitor YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[BATTERY MONITOR] Now fetching the BatteryMonitor YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[BATTERY MONITOR] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchBatteryMonitorYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+
+
+
+
+void fetchBatteryMonitorYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // BatteryMonitor.yaml
+
+ // Add the "BatteryMonitor" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "BatteryMonitor");
+
+
+
+ // Frequency of requesting the battery voltage, in [milliseconds]
+ //yaml_battery_polling_period = getParameterFloat(nodeHandle_for_paramaters,"battery_polling_period");
+
+ // Battery thresholds while in the "motors off" state, in [Volts]
+ yaml_battery_voltage_threshold_lower_while_standby = getParameterFloat(nodeHandle_for_paramaters,"battery_voltage_threshold_lower_while_standby");
+ yaml_battery_voltage_threshold_upper_while_standby = getParameterFloat(nodeHandle_for_paramaters,"battery_voltage_threshold_upper_while_standby");
+
+ // Battery thresholds while in the "flying" state, in [Volts]
+ yaml_battery_voltage_threshold_lower_while_flying = getParameterFloat(nodeHandle_for_paramaters,"battery_voltage_threshold_lower_while_flying");
+ yaml_battery_voltage_threshold_upper_while_flying = getParameterFloat(nodeHandle_for_paramaters,"battery_voltage_threshold_upper_while_flying");
+
+ // Delay before changing the state of the battery, in [number of measurements]
+ // > Note that the delay in seconds therefore depends on the polling period
+ yaml_battery_delay_threshold_to_change_state = getParameterInt(nodeHandle_for_paramaters,"battery_delay_threshold_to_change_state");
+
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[BATTERY MONITOR] DEBUGGING: the fetched BatteryMonitor/battery_voltage_threshold_lower_while_flying = " << yaml_battery_voltage_threshold_lower_while_flying);
+
+
+
+ // PROCESS ANY OF THE FETCHED PARAMETERS AS NECESSARY
+}
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+int main(int argc, char* argv[])
+{
+ // Starting the ROS-node
+ ros::init(argc, argv, "BatteryMonitor");
+
+ // 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 "BatteryMonitor" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[BATTERY MONITOR] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[BATTERY MONITOR] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[BATTERY MONITOR] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[BATTERY MONITOR] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[BATTERY MONITOR] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber batteryMonitor_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "BatteryMonitor", 1, isReadyBatteryMonitorYamlCallback);
+ ros::Subscriber batteryMonitor_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("BatteryMonitor", 1, isReadyBatteryMonitorYamlCallback);
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ fetchBatteryMonitorYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+
+
+ // PUBLISHERS
+
+ // Publisher for the filtered battery voltage
+ filteredBatteryVoltagePublisher = nodeHandle.advertise<std_msgs::Float32>("FilteredVoltage",1);
+
+ // Publisher for the battery level
+ batteryLevelPublisher = nodeHandle.advertise<std_msgs::Int32>("Level",1);
+
+ // Publisher for changes in the battery state
+ batteryStateChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("ChangedStateTo",1);
+
+
+
+ // SUBSCRIBERS
+
+ // Get a node handle to the Crazy Radio
+ std::string namespace_to_crazyradio = m_namespace + "/CrazyRadio";
+ ros::NodeHandle nodeHandle_to_crazyradio(namespace_to_crazyradio);
+
+ // Get a node handle to the Flying Agent Client
+ std::string namespace_to_FlyingAgentClient = m_namespace + "/FlyingAgentClient";
+ ros::NodeHandle nodeHandle_to_FlyingAgentClient(namespace_to_FlyingAgentClient);
+
+ // Subscribe to the voltage of the battery
+ ros::Subscriber newBatteryVoltageSubscriber = nodeHandle_to_crazyradio.subscribe("CFBattery", 1, newBatteryVoltageCallback);
+
+ // Subscribe to the status of the Crazyradio: connected, connecting or disconnected
+ ros::Subscriber crazyRadioStatusSubscriber = nodeHandle_to_crazyradio.subscribe("CrazyRadioStatus", 1, crazyRadioStatusCallback);
+
+ // Subscribe to the flying state of the agent
+ ros::Subscriber agentOperatingStateSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("flyingState", 1, agentOperatingStateCallback);
+
+ // Initialise the operating state
+ m_agent_operating_state = AGENT_OPERATING_STATE_MOTORS_OFF;
+
+
+
+ // Inform the user the this node is ready
+ ROS_INFO("[BATTERY MONITOR] Ready :-)");
+ // Spin as a single-thread node
+ ros::spin();
+
+ return 0;
+}
diff --git a/pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CentralManagerService.cpp
similarity index 91%
rename from pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp
rename to dfall_ws/src/dfall_pkg/src/nodes/CentralManagerService.cpp
index 0034c54e0751c3d280e52dff4ec5aa433d328ab7..375766ad1fe9eaa7d255eddbec9fe35c26b64d16 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CentralManagerService.cpp
@@ -157,6 +157,9 @@ bool cmCommand(CMCommand::Request &request, CMCommand::Response &response)
case CMD_SAVE:
{
writeCrazyflieDB(crazyflieDB);
+ std_msgs::Int32 msg;
+ msg.data = 1;
+ m_databaseChangedPublisher.publish(msg);
return true;
}
@@ -164,6 +167,9 @@ bool cmCommand(CMCommand::Request &request, CMCommand::Response &response)
{
crazyflieDB.crazyflieEntries.clear();
readCrazyflieDB(crazyflieDB);
+ std_msgs::Int32 msg;
+ msg.data = 1;
+ m_databaseChangedPublisher.publish(msg);
return true;
}
@@ -195,14 +201,8 @@ int main(int argc, char* argv[])
ros::ServiceServer updateService = nodeHandle.advertiseService("Update", cmUpdate);
ros::ServiceServer commandService = nodeHandle.advertiseService("Command", cmCommand);
- // Get the handle to the namespace in which this coordinator is launched
- //ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
-
- // Subscriber for requests that the controller parameters should be re-loaded from
- // the .YAML files on the coordinators machine, and then all the agents should be
- // request to fetch the parameters from itself, i.e., fetch parameters from the
- // coordinator.
- //ros::Subscriber controllerYamlReadyForFetchSubscriber = namespaceNodeHandle.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, controllerYamlReadyForFetchCallback);
+ // Publisher for when the database is saved
+ m_databaseChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
ROS_INFO("CentralManagerService ready");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d19321f1cfb08ad5876d75dcb1393779a94f9b9b
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -0,0 +1,1934 @@
+// Copyright (C) 2019, 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 fall-back controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/DefaultControllerService.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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// RRRR EEEEE QQQ U U EEEEE SSSS TTTTT
+// R R E Q Q U U E S T
+// RRRR EEE Q Q U U EEE SSS T
+// R R E Q Q U U E S T
+// R R EEEEE QQ Q UUU EEEEE SSSS T
+//
+// M M A N N OOO EEEEE U U V V RRRR EEEEE
+// MM MM A A NN N O O E U U V V R R E
+// M M M A A N N N O O EEE U U V V RRRR EEE
+// M M AAAAA N NN O O E U U V V R R E
+// M M A A N N OOO EEEEE UUU V R R EEEEE
+// ------------------------------------------------------------------------------
+
+// CALLBACK FOR THE REQUEST MANOEUVRE SERVICE
+bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Response &response)
+{
+ // Extract the requested manoeuvre
+ int requestedManoeuvre = request.data;
+
+ // Switch between the possible manoeuvres
+ switch (requestedManoeuvre)
+ {
+ case DEFAULT_CONTROLLER_REQUEST_TAKEOFF:
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Received request to perform take-off manoeuvre. Switch to state: take-off spin motors");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_TAKEOFF_SPIN_MOTORS;
+ m_current_state_changed = true;
+ // Fill in the response duration in milliseconds
+ response.data = int(
+ 1000.0 * (
+ + yaml_takoff_spin_motors_time
+ + yaml_takoff_move_up_time
+ + yaml_takoff_goto_setpoint_max_time
+ + yaml_takoff_integrator_on_time
+ )
+ );
+ break;
+ }
+
+ case DEFAULT_CONTROLLER_REQUEST_LANDING:
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Received request to perform landing manoeuvre. Switch to state: landing move down");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN;
+ m_current_state_changed = true;
+ // Fill in the response duration in milliseconds
+ response.data = int(
+ 1000 * (
+ + yaml_landing_move_down_time_max
+ + yaml_landing_spin_motors_time
+ )
+ );
+ break;
+ }
+
+ default:
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] The requested manoeuvre is not recognised. Hence switching to stand-by state.");
+ // Update the state to standby
+ m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
+ m_current_state_changed = true;
+ // Fill in the response duration in milliseconds
+ response.data = 0;
+ break;
+ }
+ }
+
+ // Publish the change
+ publishCurrentSetpointAndState();
+
+ // Return success
+ return true;
+}
+
+
+// ------------------------------------------------------------------------------
+// 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
+// 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
+//
+// L OOO OOO PPPP
+// L O O O O P P
+// L O O O O PPPP
+// L O O O O P
+// LLLLL OOO OOO P
+// ----------------------------------------------------------------------------------
+
+
+
+// THE MAIN CONTROL FUNCTION CALLED FROM THE FLYING AGENT CLIENT
+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
+
+ // Increment time
+ m_time_in_seconds += m_control_deltaT;
+
+
+ // PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
+ // > After this function is complete the class variable
+ // "m_current_stateInertialEstimate" is updated and ready
+ // to be used for subsequent controller copmutations
+ performEstimatorUpdate_forStateInterial(request);
+
+
+ // Switch between the possible states
+ switch (m_current_state)
+ {
+ case DEFAULT_CONTROLLER_STATE_NORMAL:
+ computeResponse_for_normal(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_SPIN_MOTORS:
+ computeResponse_for_takeoff_spin_motors(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_MOVE_UP:
+ computeResponse_for_takeoff_move_up(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT:
+ computeResponse_for_takeoff_goto_setpoint(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_TAKEOFF_INTEGRATOR_ON:
+ computeResponse_for_takeoff_integrator_on(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN:
+ computeResponse_for_landing_move_down(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS:
+ computeResponse_for_landing_spin_motors(response);
+ break;
+
+ case DEFAULT_CONTROLLER_STATE_STANDBY:
+ case DEFAULT_CONTROLLER_STATE_UNKNOWN:
+ default:
+ computeResponse_for_standby(response);
+ break;
+ }
+
+
+ // Change to standby state if the {roll,pitch}
+ // angles exceed the threshold
+ if (
+ (m_current_stateInertialEstimate[6] > yaml_threshold_roll_pitch_for_turn_off_radians)
+ or
+ (m_current_stateInertialEstimate[6] < -yaml_threshold_roll_pitch_for_turn_off_radians)
+ or
+ (m_current_stateInertialEstimate[7] > yaml_threshold_roll_pitch_for_turn_off_radians)
+ or
+ (m_current_stateInertialEstimate[7] < -yaml_threshold_roll_pitch_for_turn_off_radians)
+ )
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Angle thereshold exceeded. Switch to state: standby");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
+ m_current_state_changed = true;
+ // Publish a command to the "Flying Agent Client"
+ // requesting the "MOTORS-OFF" state
+ publish_motors_off_to_flying_agent_client();
+ }
+
+
+ // If the state changed,
+ // then publish the setpoint so that the GUI is updated
+ if (m_current_state_changed)
+ {
+ publishCurrentSetpointAndState();
+ }
+
+
+ // PUBLISH THE DEBUG MESSAGE (if required)
+ if (yaml_shouldPublishDebugMessage)
+ {
+ construct_and_publish_debug_message(request,response);
+ }
+
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+}
+
+
+void computeResponse_for_standby(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Nothing to perform for this state
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ }
+
+ // Fill in zero for the angle parts
+ response.controlOutput.roll = 0.0;
+ response.controlOutput.pitch = 0.0;
+ response.controlOutput.yaw = 0.0;
+
+ // Fill in all motor thrusts as zero
+ response.controlOutput.motorCmd1 = 0.0;
+ response.controlOutput.motorCmd2 = 0.0;
+ response.controlOutput.motorCmd3 = 0.0;
+ response.controlOutput.motorCmd4 = 0.0;
+
+ // Specify that using a "motor type" of command
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+}
+
+
+void computeResponse_for_normal(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Set the "m_setpoint_for_controller" variable
+ // to the current inertial estimate
+ m_setpoint_for_controller[0] = m_current_stateInertialEstimate[0];
+ m_setpoint_for_controller[1] = m_current_stateInertialEstimate[1];
+ m_setpoint_for_controller[2] = m_current_stateInertialEstimate[2];
+ m_setpoint_for_controller[3] = m_current_stateInertialEstimate[8];
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ // Inform the user
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] state \"normal\" started with \"m_setpoint_for_controller\" (x,y,z,yaw) = ( " << m_setpoint_for_controller[0] << ", " << m_setpoint_for_controller[1] << ", " << m_setpoint_for_controller[2] << ", " << m_setpoint_for_controller[3] << ")");
+ }
+
+ // Smooth out any setpoint changes
+ smoothSetpointChanges( m_setpoint , m_setpoint_for_controller );
+
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+
+}
+
+
+void computeResponse_for_takeoff_spin_motors(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Nothing to perform for this state
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ }
+
+ // Compute the time elapsed as a proportion
+ float time_elapsed_proportion = m_time_in_seconds / yaml_takoff_spin_motors_time;
+ if (time_elapsed_proportion > 1.0)
+ time_elapsed_proportion = 1.0;
+
+ // Compute the "spinning" thrust
+ float thrust_for_spinning =
+ + 1000.0
+ + time_elapsed_proportion * yaml_takeoff_spin_motors_thrust;
+
+ // Fill in zero for the angle parts
+ response.controlOutput.roll = 0.0;
+ response.controlOutput.pitch = 0.0;
+ response.controlOutput.yaw = 0.0;
+
+ // Fill in all motor thrusts as the same
+ response.controlOutput.motorCmd1 = thrust_for_spinning;
+ response.controlOutput.motorCmd2 = thrust_for_spinning;
+ response.controlOutput.motorCmd3 = thrust_for_spinning;
+ response.controlOutput.motorCmd4 = thrust_for_spinning;
+
+ // Specify that using a "motor type" of command
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+
+ // Change to next state after specified time
+ if (m_time_in_seconds > yaml_takoff_spin_motors_time)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: take-off move up");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_TAKEOFF_MOVE_UP;
+ m_current_state_changed = true;
+ }
+}
+
+
+
+void computeResponse_for_takeoff_move_up(Controller::Response &response)
+{
+ // Initialise a static variable for the starting height and yaw
+ static float initial_height = 0.0;
+ static float initial_yaw = 0.0;
+ // Initialise a static variable for the yaw change
+ static float yaw_start_to_end_diff = 0.0;
+
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Set the current (x,y) location as the setpoint
+ m_setpoint_for_controller[0] = m_current_stateInertialEstimate[0];
+ m_setpoint_for_controller[1] = m_current_stateInertialEstimate[1];
+ // Store the current (z,yaw)
+ initial_height = m_current_stateInertialEstimate[2];
+ initial_yaw = m_current_stateInertialEstimate[8];
+ // Put these back into the setpoint
+ m_setpoint_for_controller[2] = initial_height + yaml_takeoff_move_up_start_height;
+ m_setpoint_for_controller[3] = initial_yaw;
+ // Compute the yaw "start-to-end-difference" unwrapped
+ yaw_start_to_end_diff = m_setpoint[3] - initial_yaw;
+ while(yaw_start_to_end_diff > PI) {yaw_start_to_end_diff -= 2 * PI;}
+ while(yaw_start_to_end_diff < -PI) {yaw_start_to_end_diff += 2 * PI;}
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ // Inform the user
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] state \"take-off move-up\" started with \"m_setpoint_for_controller\" (x,y,z,yaw) = ( " << m_setpoint_for_controller[0] << ", " << m_setpoint_for_controller[1] << ", " << m_setpoint_for_controller[2] << ", " << m_setpoint_for_controller[3] << ")");
+ }
+
+ // Compute the time elapsed as a proportion
+ float time_elapsed_proportion = m_time_in_seconds / (0.8*yaml_takoff_move_up_time);
+ if (time_elapsed_proportion > 1.0)
+ time_elapsed_proportion = 1.0;
+
+ // Compute the z-height setpoint
+ m_setpoint_for_controller[2] = initial_height + yaml_takeoff_move_up_start_height + time_elapsed_proportion * (yaml_takeoff_move_up_end_height-yaml_takeoff_move_up_start_height);
+
+ // Compute the yaw-setpoint
+ m_setpoint_for_controller[3] = initial_yaw + time_elapsed_proportion * yaw_start_to_end_diff;
+
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+
+ // Change to next state after specified time
+ if (m_time_in_seconds > yaml_takoff_move_up_time)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: take-off goto setpoint");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT;
+ m_current_state_changed = true;
+ }
+}
+
+
+
+void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response)
+{
+ // Initialise a static variable for the time
+ // to switch after entering the "box" around
+ // the setpoint
+ static float time_to_switch = 10.0;
+ static bool box_reached = false;
+
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Initialise the time to switch as greater than the
+ // max time
+ time_to_switch = yaml_takoff_goto_setpoint_max_time + yaml_takoff_goto_setpoint_nearby_time;
+ // Initialise the bool for whether the box was reached
+ box_reached = false;
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ // Inform the user
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] state \"take-off goto-setpoint\" started with \"m_setpoint\" (x,y,z,yaw) = ( " << m_setpoint[0] << ", " << m_setpoint[1] << ", " << m_setpoint[2] << ", " << m_setpoint[3] << ")");
+ }
+
+ // Smooth out any setpoint changes
+ smoothSetpointChanges( m_setpoint , m_setpoint_for_controller );
+
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+
+ // Check if within the "integrator on" box of the setpoint
+ // > First, compute the current errors
+ float abs_error_x = m_setpoint[0] - m_current_stateInertialEstimate[0];
+ float abs_error_y = m_setpoint[1] - m_current_stateInertialEstimate[1];
+ float abs_error_z = m_setpoint[2] - m_current_stateInertialEstimate[2];
+ if (abs_error_x<0.0){abs_error_x=-abs_error_x;}
+ if (abs_error_y<0.0){abs_error_y=-abs_error_y;}
+ if (abs_error_z<0.0){abs_error_z=-abs_error_z;}
+ // > Then perform the check
+ if (
+ (!box_reached)
+ and
+ (abs_error_x < yaml_takoff_integrator_on_box_horizontal)
+ and
+ (abs_error_y < yaml_takoff_integrator_on_box_horizontal)
+ and
+ (abs_error_z < yaml_takoff_integrator_on_box_vertical)
+ )
+ {
+ // Give it another "yaml_takoff_goto_setpoint_nearby_time"
+ // seconds before changing to the integrator
+ time_to_switch = m_time_in_seconds + yaml_takoff_goto_setpoint_nearby_time;
+ // Set the bool that the box was reached
+ box_reached = true;
+ }
+
+ if (m_time_in_seconds > time_to_switch)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: take-off integrator on");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_TAKEOFF_INTEGRATOR_ON;
+ m_current_state_changed = true;
+ }
+
+ // Change to normal if the timeout is reched
+ if (m_time_in_seconds > yaml_takoff_goto_setpoint_max_time)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Did not reached the setpoint within the \"take-off goto setpoint\" allowed time. Switch to state: normal");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_NORMAL;
+ m_current_state_changed = true;
+ }
+}
+
+
+
+void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Set the "m_setpoint_for_controller" variable
+ // to the current setpoint
+ m_setpoint_for_controller[0] = m_setpoint[0];
+ m_setpoint_for_controller[1] = m_setpoint[1];
+ m_setpoint_for_controller[2] = m_setpoint[2];
+ m_setpoint_for_controller[3] = m_setpoint[3];
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ // Inform the user
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] state \"take-off integrator-on\" started with \"m_setpoint\" (x,y,z,yaw) = ( " << m_setpoint[0] << ", " << m_setpoint[1] << ", " << m_setpoint[2] << ", " << m_setpoint[3] << ")");
+ }
+
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response , INTEGRATOR_FLAG_ON);
+
+ // Change to next state after specified time
+ if (m_time_in_seconds > yaml_takoff_integrator_on_time)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: normal");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_NORMAL;
+ m_current_state_changed = true;
+ }
+}
+
+
+
+
+
+void computeResponse_for_landing_move_down(Controller::Response &response)
+{
+ // Initialise a static variable for the starting height and yaw
+ static float target_landing_setpoint[4] = {0.0,0.0,0.4,0.0};
+
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Set the current (x,y,z,yaw) location as the setpoint
+ m_setpoint_for_controller[0] = m_current_stateInertialEstimate[0];
+ m_setpoint_for_controller[1] = m_current_stateInertialEstimate[1];
+ m_setpoint_for_controller[2] = m_current_stateInertialEstimate[2];
+ m_setpoint_for_controller[3] = m_current_stateInertialEstimate[8];
+ // Make the target setpoint the same for (x,y,yaw)
+ target_landing_setpoint[0] = m_setpoint_for_controller[0];
+ target_landing_setpoint[1] = m_setpoint_for_controller[1];
+ target_landing_setpoint[3] = m_setpoint_for_controller[3];
+ // Set the target height
+ target_landing_setpoint[2] = yaml_landing_move_down_end_height_setpoint;
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ // Inform the user
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] state \"landing move-down\" started with \"m_setpoint_for_controller\" (x,y,z,yaw) = ( " << m_setpoint_for_controller[0] << ", " << m_setpoint_for_controller[1] << ", " << m_setpoint_for_controller[2] << ", " << m_setpoint_for_controller[3] << ")");
+ }
+
+ // Smooth out any setpoint changes
+ smoothSetpointChanges( target_landing_setpoint , m_setpoint_for_controller );
+
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+
+ // Check if within the threshold of zero
+ if (m_current_stateInertialEstimate[2] < yaml_landing_move_down_end_height_threshold)
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: landing spin motors");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS;
+ m_current_state_changed = true;
+ }
+
+ // Change to landing spin motors if the timeout is reached
+ if ( m_time_in_seconds > yaml_landing_move_down_time_max )
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Did not reached the setpoint within the \"landing move down\" allowed time. Switch to state: landing spin motors");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS;
+ m_current_state_changed = true;
+ }
+}
+
+
+void computeResponse_for_landing_spin_motors(Controller::Response &response)
+{
+ // Check if the state "just recently" changed
+ if (m_current_state_changed)
+ {
+ // PERFORM "ONE-OFF" OPERATIONS HERE
+ // Set the z setpoint
+ m_setpoint_for_controller[2] = yaml_landing_move_down_end_height_setpoint;
+ // Set the change flag back to false
+ m_current_state_changed = false;
+ // Inform the user
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] state \"landing spin motors\" started with \"m_setpoint_for_controller\" (x,y,z,yaw) = ( " << m_setpoint_for_controller[0] << ", " << m_setpoint_for_controller[1] << ", " << m_setpoint_for_controller[2] << ", " << m_setpoint_for_controller[3] << ")");
+ }
+
+ // Compute the time elapsed as a proportion
+ float time_elapsed_proportion = m_time_in_seconds / yaml_landing_spin_motors_time;
+ if (time_elapsed_proportion > 1.0)
+ time_elapsed_proportion = 1.0;
+
+
+ // Start by using the controller and reducing the thrust
+ if (time_elapsed_proportion<0.5)
+ {
+ // Call the LQR control function
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, INTEGRATOR_FLAG_OFF);
+ // Compute the desired "spinning" thrust
+ float thrust_for_spinning =
+ (1.0-time_elapsed_proportion)
+ *
+ computeMotorPolyBackward(m_cf_weight_in_newtons/4.0);
+ // Adjust the motor commands
+ response.controlOutput.motorCmd1 = thrust_for_spinning;
+ response.controlOutput.motorCmd2 = thrust_for_spinning;
+ response.controlOutput.motorCmd3 = thrust_for_spinning;
+ response.controlOutput.motorCmd4 = thrust_for_spinning;
+ }
+ // Next stop using the controller and just spin the motors
+ else
+ {
+ // Fill in zero for the angle parts
+ response.controlOutput.roll = 0.0;
+ response.controlOutput.pitch = 0.0;
+ response.controlOutput.yaw = 0.0;
+ // Fill in all motor thrusts as the same
+ response.controlOutput.motorCmd1 = yaml_landing_spin_motors_thrust;
+ response.controlOutput.motorCmd2 = yaml_landing_spin_motors_thrust;
+ response.controlOutput.motorCmd3 = yaml_landing_spin_motors_thrust;
+ response.controlOutput.motorCmd4 = yaml_landing_spin_motors_thrust;
+ // Specify that using a "motor type" of command
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+
+ // Change to next state after specified time
+ if ( m_time_in_seconds > (0.7*yaml_landing_spin_motors_time) )
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Switch to state: standby");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
+ m_current_state_changed = true;
+ }
+ }
+}
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// SSSS M M OOO OOO TTTTT H H
+// S MM MM O O O O T H H
+// SSS M M M O O O O T HHHHH
+// S M M O O O O T H H
+// SSSS M M OOO OOO T H H
+//
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+// ------------------------------------------------------------------------------
+
+
+void smoothSetpointChanges( float target_setpoint[4] , float (¤t_setpoint)[4] )
+{
+ // NO SMOOTHING IS APPLIED TO THE YAW-COORDINATE
+ // > Hence copy it across directly
+ current_setpoint[3] = target_setpoint[3];
+
+ // SMOOTH THE Z-COORIDINATE
+ // > Compute the max allowed change
+ float max_for_z = yaml_max_setpoint_change_per_second_vertical / yaml_control_frequency;
+ // > Compute the current difference
+ float diff_for_z = target_setpoint[2] - current_setpoint[2];
+ // > Clip the difference to the maximum
+ if (diff_for_z > max_for_z)
+ diff_for_z = max_for_z;
+ else if (diff_for_z < -max_for_z)
+ diff_for_z = -max_for_z;
+ // > Update the current setpoint
+ current_setpoint[2] += diff_for_z;
+
+ // SMOOTH THE X-Y-COORIDINATES
+ // > Compute the max allowed change
+ float max_for_xy = yaml_max_setpoint_change_per_second_horizontal / yaml_control_frequency;
+ // > Compute the current difference
+ float diff_for_x = target_setpoint[0] - current_setpoint[0];
+ float diff_for_y = target_setpoint[1] - current_setpoint[1];
+ float diff_for_xy = sqrt( diff_for_x*diff_for_x + diff_for_y*diff_for_y );
+ // > Clip the difference to the maximum
+ if (diff_for_xy > max_for_xy)
+ {
+ // > Convert the difference to a proportion
+ float proportion_xy = max_for_xy / diff_for_xy;
+ // > Update the current setpoint
+ current_setpoint[0] += proportion_xy * diff_for_x;
+ current_setpoint[1] += proportion_xy * diff_for_y;
+ }
+ else
+ {
+ // > Update the current setpoint to be the
+ // the target setpoint because it is within
+ // reach
+ current_setpoint[0] = target_setpoint[0];
+ current_setpoint[1] = target_setpoint[1];
+ }
+}
+
+
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// 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
+ m_current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ m_current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ m_current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ m_current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ m_current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ m_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 (yaml_estimator_method)
+ {
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 9; ++i)
+ {
+ m_current_stateInertialEstimate[i] = m_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 < 9; ++i)
+ {
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "yaml_estimator_method" was not recognised
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DefaultControllerService': the 'yaml_estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default
+ for(int i = 0; i < 9; ++i)
+ {
+ m_current_stateInertialEstimate[i] = m_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
+ m_previous_xzy_rpy_measurement[0] = m_current_xzy_rpy_measurement[0];
+ m_previous_xzy_rpy_measurement[1] = m_current_xzy_rpy_measurement[1];
+ m_previous_xzy_rpy_measurement[2] = m_current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ m_previous_xzy_rpy_measurement[3] = m_current_xzy_rpy_measurement[3];
+ m_previous_xzy_rpy_measurement[4] = m_current_xzy_rpy_measurement[4];
+ m_previous_xzy_rpy_measurement[5] = m_current_xzy_rpy_measurement[5];
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+{
+ // PUT IN THE CURRENT MEASUREMENT DIRECTLY
+ // > for (x,y,z) position
+ m_stateInterialEstimate_viaFiniteDifference[0] = m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaFiniteDifference[1] = m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaFiniteDifference[2] = m_current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ m_stateInterialEstimate_viaFiniteDifference[6] = m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaFiniteDifference[7] = m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaFiniteDifference[8] = m_current_xzy_rpy_measurement[5];
+
+ // COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
+ // > for (x,y,z) velocities
+ m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+{
+ // PERFORM THE KALMAN FILTER UPDATE STEP
+ // > First take a copy of the estimator state
+ float temp_PMKFstate[9];
+ for(int i = 0; i < 9; ++i)
+ {
+ temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // > Now perform update for:
+ // > x position and velocity:
+ m_stateInterialEstimate_viaPointMassKalmanFilter[0] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[3] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[0];
+ // > y position and velocity:
+ m_stateInterialEstimate_viaPointMassKalmanFilter[1] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[4] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[1];
+ // > z position and velocity:
+ m_stateInterialEstimate_viaPointMassKalmanFilter[2] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[2];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[5] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[2];
+
+ // > for (roll,pitch,yaw) angles
+ // (taken directly from the measurement):
+ m_stateInterialEstimate_viaPointMassKalmanFilter[6] = m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[7] = m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[8] = m_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
+// ----------------------------------------------------------------------------------
+
+// > This function constructs the error in the body frame
+// before calling the appropriate control function
+void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateInertial[9], Controller::Response &response, int integrator_flag )
+{
+ // Store the current YAW in a local variable
+ float temp_stateInertial_yaw = stateInertial[8];
+
+ // Initialise an array for the state error in the inertial frame
+ float stateInertialError[9];
+
+ // Adjust the INERTIAL (x,y,z) position for the setpoint
+ stateInertialError[0] = stateInertial[0] - setpoint[0];
+ stateInertialError[1] = stateInertial[1] - setpoint[1];
+ stateInertialError[2] = stateInertial[2] - setpoint[2];
+
+ // The linear velocities can be directly copied across
+ stateInertialError[3] = stateInertial[3];
+ stateInertialError[4] = stateInertial[4];
+ stateInertialError[5] = stateInertial[5];
+
+ // The angular velocities for {roll,pitch} can be directly
+ // copied across
+ stateInertialError[6] = stateInertial[6];
+ stateInertialError[7] = stateInertial[7];
+
+ // Clip the x-coordination to within the specified bounds
+ if (stateInertialError[0] > yaml_max_setpoint_error_xy)
+ stateInertialError[0] = yaml_max_setpoint_error_xy;
+ else if (stateInertialError[0] < -yaml_max_setpoint_error_xy)
+ stateInertialError[0] = -yaml_max_setpoint_error_xy;
+
+ // Clip the y-coordination to within the specified bounds
+ if (stateInertialError[1] > yaml_max_setpoint_error_xy)
+ stateInertialError[1] = yaml_max_setpoint_error_xy;
+ else if (stateInertialError[1] < -yaml_max_setpoint_error_xy)
+ stateInertialError[1] = -yaml_max_setpoint_error_xy;
+
+ // Clip the z-coordination to within the specified bounds
+ if (stateInertialError[2] > yaml_max_setpoint_error_z)
+ stateInertialError[2] = yaml_max_setpoint_error_z;
+ else if (stateInertialError[2] < -yaml_max_setpoint_error_z)
+ stateInertialError[2] = -yaml_max_setpoint_error_z;
+
+ // Fill in the yaw angle error
+ // > This error should be "unwrapped" to be in the range
+ // ( -pi , pi )
+ // > Det the yaw error into a local variable
+ float yawError = stateInertial[8] - setpoint[3];
+ // > "Unwrap" the yaw error to the interval ( -pi , pi )
+ while(yawError > PI) {yawError -= 2 * PI;}
+ while(yawError < -PI) {yawError += 2 * PI;}
+ // Clip the error to within the specified bounds
+ if (yawError > yaml_max_setpoint_error_yaw_radians)
+ yawError = yaml_max_setpoint_error_yaw_radians;
+ else if (yawError < -yaml_max_setpoint_error_yaw_radians)
+ yawError = -yaml_max_setpoint_error_yaw_radians;
+
+ // > Finally, put the "yawError" into the "stateError" variable
+ stateInertialError[8] = yawError;
+
+ // CONVERSION INTO BODY FRAME
+ // Initialise a variable for the body frame error
+ float bodyFrameError[9];
+ // Call the function to convert the state erorr from
+ // the Inertial frame into the Body frame
+ convertIntoBodyFrame(stateInertialError, bodyFrameError, temp_stateInertial_yaw);
+
+ calculateControlOutput_viaLQR_givenError(bodyFrameError, response, integrator_flag);
+}
+
+void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controller::Response &response, int integrator_flag)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Initialise static variables for the integral
+ static float newtons_int = 0.0;
+ static float tau_x = 0.0;
+ static float tau_y = 0.0;
+ static float tau_z = 0.0;
+
+
+ // Compute the Z-CONTROLLER
+ // > provides the total thrust adjustment
+ float thrustAdjustment =
+ - yaml_gainMatrixThrust_2StateVector[0] * stateErrorBody[2]
+ - yaml_gainMatrixThrust_2StateVector[1] * stateErrorBody[5];
+
+ // Compute the YAW-CONTROLLER
+ // > provides the body frame yaw rate
+ float yawRate_forResponse =
+ - yaml_gainYawRate_fromAngle * stateErrorBody[8];
+
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ float rollRate_forResponse = 0.0;
+ float pitchRate_forResponse = 0.0;
+
+ // Switch between the differnt control method for
+ // the X-Y-CONTROLLER
+ switch (yaml_controller_method)
+ {
+ case CONTROLLER_METHOD_RATES:
+ {
+ // Compute the X-CONTROLLER
+ // > provides the body frame pitch rate
+ pitchRate_forResponse =
+ - yaml_gainMatrixPitchRate_3StateVector[0] * stateErrorBody[0]
+ - yaml_gainMatrixPitchRate_3StateVector[1] * stateErrorBody[3]
+ - yaml_gainMatrixPitchRate_3StateVector[2] * stateErrorBody[7];
+
+ // Compute the Y-CONTROLLER
+ // > provides the body frame roll rate
+ rollRate_forResponse =
+ - yaml_gainMatrixRollRate_3StateVector[0] * stateErrorBody[1]
+ - yaml_gainMatrixRollRate_3StateVector[1] * stateErrorBody[4]
+ - yaml_gainMatrixRollRate_3StateVector[2] * stateErrorBody[6];
+ break;
+ }
+
+ case CONTROLLER_METHOD_RATE_ANGLE_NESTED:
+ {
+ // Compute the X-CONTROLLER
+ // > Compute the desired pitch angle
+ float pitchAngle_desired =
+ - yaml_gainMatrixPitchAngle_2StateVector[0] * stateErrorBody[0]
+ - yaml_gainMatrixPitchAngle_2StateVector[1] * stateErrorBody[3];
+ // Clip the request to within the specified limits
+ if (pitchAngle_desired > yaml_max_roll_pitch_request_radians)
+ pitchAngle_desired = yaml_max_roll_pitch_request_radians;
+ else if (pitchAngle_desired < -yaml_max_roll_pitch_request_radians)
+ pitchAngle_desired = -yaml_max_roll_pitch_request_radians;
+ // > Compute the pitch rate
+ pitchRate_forResponse =
+ - yaml_gainPitchRate_fromAngle * (stateErrorBody[7] - pitchAngle_desired);
+
+ // Compute the Y-CONTROLLER
+ // > Compute the desired roll angle
+ float rollAngle_desired =
+ - yaml_gainMatrixRollAngle_2StateVector[0] * stateErrorBody[1]
+ - yaml_gainMatrixRollAngle_2StateVector[1] * stateErrorBody[4];
+ // Clip the request to within the specified limits
+ if (rollAngle_desired > yaml_max_roll_pitch_request_radians)
+ rollAngle_desired = yaml_max_roll_pitch_request_radians;
+ else if (rollAngle_desired < -yaml_max_roll_pitch_request_radians)
+ rollAngle_desired = -yaml_max_roll_pitch_request_radians;
+ // > Compute the roll rate
+ rollRate_forResponse =
+ - yaml_gainRollRate_fromAngle * (stateErrorBody[6] - rollAngle_desired);
+
+ break;
+ }
+
+ default:
+ {
+ // Inform the user of the error
+ ROS_ERROR("[DEFAULT CONTROLLER] The variable \"yaml_controller_method\" is not recognised.");
+ break;
+ }
+ }
+
+
+ // PERFORM THE INTEGRATOR COMPUTATIONS
+ if (integrator_flag == INTEGRATOR_FLAG_ON)
+ {
+ newtons_int -= yaml_integratorGain_forThrust * stateErrorBody[2] / yaml_control_frequency;
+ tau_x += yaml_integratorGain_forTauXY * stateErrorBody[1] / yaml_control_frequency;
+ tau_y -= yaml_integratorGain_forTauXY * stateErrorBody[0] / yaml_control_frequency;
+ tau_z -= yaml_integratorGain_forTauYaw * stateErrorBody[8] / yaml_control_frequency;
+ }
+ else if (integrator_flag == INTEGRATOR_FLAG_RESET)
+ {
+ newtons_int = 0.0;
+ tau_x = 0.0;
+ tau_y = 0.0;
+ tau_z = 0.0;
+ }
+
+ // 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;
+ // > Compute the feed-forward force
+ float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0 + newtons_int;
+ // > Put in the per motor commands
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor - tau_x - tau_y - tau_z);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor - tau_x + tau_y + tau_z);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor + tau_x + tau_y - tau_z);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor + tau_x - tau_y + tau_z);
+
+
+ // 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 (yaml_shouldDisplayDebugInfo)
+ {
+ // 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:" << yaml_motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 2:" << yaml_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);
+ }
+}
+
+
+
+// ***********************************************************
+// 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
+
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response)
+{
+
+ // 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"
+ m_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
+// ----------------------------------------------------------------------------------
+
+// ROTATES THE (x,y) COMPONENTS BY THE PROVIDED "yaw" ANGLE
+void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
+{
+
+ 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];
+}
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+// CONVERTS A THURST IN NEWTONS TO A 16-BIT NUMBER
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-bit command that would produce the requested
+ // "thrust" based on the quadratic mapping that is described
+ // by the coefficients in the "yaml_motorPoly" variable.
+ float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd_16bit < yaml_command_sixteenbit_min)
+ {
+ cmd_16bit = 0;
+ }
+ else if (cmd_16bit > yaml_command_sixteenbit_max)
+ {
+ cmd_16bit = yaml_command_sixteenbit_max;
+ }
+
+ // Return the result
+ return cmd_16bit;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Check if the request if for the default setpoint
+ if (newSetpoint.buttonID == REQUEST_DEFAULT_SETPOINT_BUTTON_ID)
+ {
+ setNewSetpoint(
+ yaml_default_setpoint[0],
+ yaml_default_setpoint[1],
+ yaml_default_setpoint[2],
+ yaml_default_setpoint[3]
+ );
+ }
+ else
+ {
+ // Call the function for actually setting the setpoint
+ setNewSetpoint(
+ newSetpoint.x,
+ newSetpoint.y,
+ newSetpoint.z,
+ newSetpoint.yaw
+ );
+ }
+ }
+}
+
+
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw)
+{
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = x;
+ m_setpoint[1] = y;
+ m_setpoint[2] = z;
+ m_setpoint[3] = yaw;
+
+ // Publish the change so that the network is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
+
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response)
+{
+ // Directly put the current setpoint into the response
+ response.setpointWithHeader.x = m_setpoint[0];
+ response.setpointWithHeader.y = m_setpoint[1];
+ response.setpointWithHeader.z = m_setpoint[2];
+ response.setpointWithHeader.yaw = m_setpoint[3];
+ // Put the current state into the "buttonID" field
+ response.setpointWithHeader.buttonID = m_current_state;
+ // Return
+ return true;
+}
+
+
+// PUBLISH THE CURRENT SETPOINT SO THAT THE NETWORK IS UPDATED
+void publishCurrentSetpointAndState()
+{
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.x = m_setpoint[0];
+ msg.y = m_setpoint[1];
+ msg.z = m_setpoint[2];
+ msg.yaw = m_setpoint[3];
+ // Put the current state into the "buttonID" field
+ msg.buttonID = m_current_state;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 CustomButtonWithHeader& commandReceived)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , commandReceived.shouldCheckForAgentID , commandReceived.agentIDs );
+
+ if (isRevelant)
+ {
+ // Extract the data from the message
+ int custom_button_index = commandReceived.button_index;
+ float custom_command_code = commandReceived.float_data;
+
+ // Switch between the button pressed
+ switch(custom_button_index)
+ {
+
+ // > FOR CUSTOM BUTTON 1
+ case 1:
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Received request to run integrator. Switch to state: take-off goto-setpoint");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_TAKEOFF_GOTO_SETPOINT;
+ m_current_state_changed = true;
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 2
+ case 2:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO("[DEFAULT CONTROLLER] Received request to reset the integrator.");
+ // Call the controller function with the reset flag
+ float tempStateBodyError[9];
+ Controller::Response temp_response;
+ calculateControlOutput_viaLQR_givenError(tempStateBodyError, temp_response, INTEGRATOR_FLAG_RESET);
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 3
+ case 3:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[DEFAULT 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;
+ }
+ }
+ }
+}
+
+
+
+
+
+
+// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
+void publish_motors_off_to_flying_agent_client()
+{
+ // Instantiate a local variable of type "IntWithHeader"
+ IntWithHeader msg;
+ // Fill in the MOTORS-OFF command
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ // Fill in the header that this applies
+ msg.shouldCheckForAgentID = false;
+ // Publish the message
+ m_motorsOffToFlyingAgentClientPublisher.publish(msg);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// TIMER CALLBACK FOR SENDING THE LOAD YAML REQUEST
+void timerCallback_initial_load_yaml(const ros::TimerEvent&)
+{
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "DefaultController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyDefaultControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[DEFAULT CONTROLLER] The request load yaml file service call failed.");
+ }
+}
+
+
+// LOADING OF YAML PARAMETERS
+void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[DEFAULT CONTROLLER] Now fetching the DefaultController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[DEFAULT CONTROLLER] Now fetching the DefaultController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[DEFAULT CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchDefaultControllerYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+// LOADING OF YAML PARAMETERS
+void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // DefaultController.yaml
+
+ // Add the "DefaultController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "DefaultController");
+
+
+
+ // GET THE PARAMETERS:
+
+ // ------------------------------------------------------
+ // PARAMTERS FOR THE TAKE-OFF AND LANDING MANOEUVRES
+
+ // Max setpoint change per second
+ yaml_max_setpoint_change_per_second_horizontal = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_change_per_second_horizontal");
+ yaml_max_setpoint_change_per_second_vertical = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_change_per_second_vertical");
+
+ // Max error for z
+ yaml_max_setpoint_error_z = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_z");
+
+ // Max error for xy
+ yaml_max_setpoint_error_xy = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_xy");
+
+ // Max {roll,pitch} angle request
+ yaml_max_roll_pitch_request_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_roll_pitch_request_degrees");
+
+ // Max error for yaw angle
+ yaml_max_setpoint_error_yaw_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_error_yaw_degrees");
+
+ // Theshold for {roll,pitch} angle beyond
+ // which the motors are turned off
+ yaml_threshold_roll_pitch_for_turn_off_degrees = getParameterFloat(nodeHandle_for_paramaters , "threshold_roll_pitch_for_turn_off_degrees");
+
+ // The thrust for take off spin motors
+ yaml_takeoff_spin_motors_thrust = getParameterFloat(nodeHandle_for_paramaters , "takeoff_spin_motors_thrust");
+ // The time for the take off spin(-up) motors
+ yaml_takoff_spin_motors_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_spin_motors_time");
+
+ // Height change for the take off move-up
+ yaml_takeoff_move_up_start_height = getParameterFloat(nodeHandle_for_paramaters , "takeoff_move_up_start_height");
+ yaml_takeoff_move_up_end_height = getParameterFloat(nodeHandle_for_paramaters , "takeoff_move_up_end_height");
+ // The time for the take off spin motors
+ yaml_takoff_move_up_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_move_up_time");
+
+ // Minimum and maximum allowed time for: take off goto setpoint
+ yaml_takoff_goto_setpoint_nearby_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_goto_setpoint_nearby_time");
+ yaml_takoff_goto_setpoint_max_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_goto_setpoint_max_time");
+
+ // Box within which to keep the integrator on
+ // > Units of [meters]
+ // > The box consider is plus/minus this value
+ yaml_takoff_integrator_on_box_horizontal = getParameterFloat(nodeHandle_for_paramaters , "takoff_integrator_on_box_horizontal");
+ yaml_takoff_integrator_on_box_vertical = getParameterFloat(nodeHandle_for_paramaters , "takoff_integrator_on_box_vertical");
+ // The time for: take off integrator-on
+ yaml_takoff_integrator_on_time = getParameterFloat(nodeHandle_for_paramaters , "takoff_integrator_on_time");
+
+ // Height change for the landing move-down
+ yaml_landing_move_down_end_height_setpoint = getParameterFloat(nodeHandle_for_paramaters , "landing_move_down_end_height_setpoint");
+ yaml_landing_move_down_end_height_threshold = getParameterFloat(nodeHandle_for_paramaters , "landing_move_down_end_height_threshold");
+ // The time for: landing move-down
+ yaml_landing_move_down_time_max = getParameterFloat(nodeHandle_for_paramaters , "landing_move_down_time_max");
+
+ // The thrust for landing spin motors
+ yaml_landing_spin_motors_thrust = getParameterFloat(nodeHandle_for_paramaters , "landing_spin_motors_thrust");
+ // The time for: landing spin motors
+ yaml_landing_spin_motors_time = getParameterFloat(nodeHandle_for_paramaters , "landing_spin_motors_time");
+
+
+
+
+ // ------------------------------------------------------
+ // PARAMTERS THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
+
+ // The mass of the crazyflie, in [grams]
+ yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
+
+ // > The frequency at which the "computeControlOutput" function
+ // is being called, as determined by the frequency at which
+ // the Motion Caption (Vicon) system provides pose data, i.e.,
+ // measurement of (x,y,z) position and (roll,pitch,yaw) attitude.
+ yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit
+ // motor command to thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+
+ // The min and max for saturating 16 bit thrust commands
+ yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+ yaml_command_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_max");
+
+ // The default setpoint, the ordering is (x,y,z,yaw),
+ // with unit [meters,meters,meters,radians]
+ getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+
+ // Boolean indiciating whether the "Debug Message" of this agent should be published or not
+ yaml_shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paramaters, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+ yaml_shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
+
+ // > A flag for which controller to use:
+ yaml_controller_method = getParameterInt( nodeHandle_for_paramaters , "controller_method" );
+
+ // The LQR Controller parameters for z-height
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_2StateVector", yaml_gainMatrixThrust_2StateVector, 2);
+ // The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_3StateVector", yaml_gainMatrixRollRate_3StateVector, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_3StateVector", yaml_gainMatrixPitchRate_3StateVector, 3);
+ // The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_2StateVector", yaml_gainMatrixRollAngle_2StateVector, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle_2StateVector", yaml_gainMatrixPitchAngle_2StateVector, 2);
+ yaml_gainRollRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainRollRate_fromAngle");
+ yaml_gainPitchRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainPitchRate_fromAngle");
+ // The LQR Controller parameters for yaw
+ yaml_gainYawRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainYawRate_fromAngle");
+ // Integrator gains
+ yaml_integratorGain_forThrust = getParameterFloat(nodeHandle_for_paramaters, "integratorGain_forThrust");
+ yaml_integratorGain_forTauXY = getParameterFloat(nodeHandle_for_paramaters, "integratorGain_forTauXY");
+ yaml_integratorGain_forTauYaw = getParameterFloat(nodeHandle_for_paramaters, "integratorGain_forTauYaw");
+
+
+ // A flag for which estimator to use:
+ yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
+
+ // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+ // > For the (x,y,z) position
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
+
+
+
+ // > DEBUGGING: Print out one of the parameters that was loaded to
+ // debug if the fetching of parameters worked correctly
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] DEBUGGING: the fetched DefaultController/mass = " << yaml_cf_mass_in_grams);
+
+
+
+ // PROCESS THE PARAMTERS
+
+ // > Compute the feed-forward force that we need to counteract
+ // gravity (i.e., mg) in units of [Newtons]
+ m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
+
+ // > Conver the control frequency to a delta T
+ m_control_deltaT = 1.0 / yaml_control_frequency;
+
+ // Max error for yaw angle
+ yaml_max_setpoint_error_yaw_radians = DEG2RAD * yaml_max_setpoint_error_yaw_degrees;
+
+ // Max {roll,pitch} angle request
+ yaml_max_roll_pitch_request_radians = DEG2RAD * yaml_max_roll_pitch_request_degrees;
+
+ // Theshold for {roll,pitch} angle beyond
+ // which the motors are turned off
+ yaml_threshold_roll_pitch_for_turn_off_radians = DEG2RAD * yaml_threshold_roll_pitch_for_turn_off_degrees;
+
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+int main(int argc, char* argv[])
+{
+ // Starting the ROS-node
+ ros::init(argc, argv, "DefaultControllerService");
+
+ // 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 "DefaultControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[DEFAULT CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber safeContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "DefaultController", 1, isReadyDefaultControllerYamlCallback);
+ ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("DefaultController", 1, isReadyDefaultControllerYamlCallback);
+
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a serice client
+ // we stall the availability of this node until the
+ // paramter service is ready
+ // > NOTE FURTHER that calling on the service directly from here
+ // often means the yaml file is not actually loaded. So we
+ // instead use a timer to delay the loading
+
+ // Create a single-shot timer
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_initial_load_yaml, true);
+ timer_initial_load_yaml.start();
+
+
+
+
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "requestSetpointChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestSetpointChange" topic and calls the class function
+ // "requestSetpointChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the setpoint is changed
+ // by the user.
+ ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("DefaultControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Instantiate the class variable "m_setpointChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "SetpointChanged" and is a message with the structure defined
+ // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current setpoint for this controller.
+ m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
+
+ // Instantiate the local variable "getCurrentSetpointService" to be
+ // a "ros::ServiceServer" type variable that advertises the service
+ // called "GetCurrentSetpoint". This service has the input-output
+ // behaviour defined in the "GetSetpointService.srv" file (located
+ // in the "srv" folder). This service, when called, is provided with
+ // an integer (that is essentially ignored), and is expected to respond
+ // with the current setpoint of the controller. When a request is made
+ // of this service the "getCurrentSetpointCallback" function is called.
+ ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);
+
+
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "DefaultController". 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 controllerService = nodeHandle.advertiseService("DefaultController", 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("CustomButtonPressed", 1, customCommandReceivedCallback);
+
+
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer"
+ // type variable that advertises the service called:
+ // >> "RequestManoeuvre"
+ // This service has the input-output behaviour defined in the
+ // "IntIntService.srv" file (located in the "srv" folder).
+ // This service, when called, is provided with what manoeuvre
+ // is requested and responds with the duration that menoeuvre
+ // will take to perform (in milliseconds)
+ ros::ServiceServer requestManoeuvreService = nodeHandle.advertiseService("RequestManoeuvre", requestManoeuvreCallback);
+
+ // Instantiate the class variable "m_motorsOffToFlyingAgentClientPublisher"
+ // to be a "ros::Publisher". This variable advertises under the
+ // name space:
+ // "FlyingAgentClient/Command"
+ // meaning that it is mimicing messages send by the "Fying
+ // Agent Client" node. The message sent has the structure
+ // defined in the file "IntWithHeader.msg".
+ // > First create a node handle to the base namespace
+ // i.e., the namespace: "/dfall/agentXXX/"
+ ros::NodeHandle base_nodeHandle(m_namespace);
+ // > Now instantiate the publisher
+ m_motorsOffToFlyingAgentClientPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/Command", 1);
+
+
+
+ // Print out some information to the user.
+ ROS_INFO("[DEFAULT 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/DemoControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp
similarity index 77%
rename from pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
rename to dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp
index a4ffa5643672ff24d4c9256b4a8e584ef1b6db21..7988825d0e6b7b62c58a5b4f8ff217bb0ad575ee 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp
@@ -113,22 +113,30 @@
// 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:
+// > The allowed values for "onboardControllerType" are in the "Defines"
+// section in the header file, they are:
+// - CF_COMMAND_TYPE_MOTORS
+// - CF_COMMAND_TYPE_RATE
+// - CF_COMMAND_TYPE_ANGLE
+//
+// > For completeing the class exercises it is only required to use
+// the CF_COMMAND_TYPE_RATE option.
+//
+// > For the CF_COMMAND_TYPE_RATE optoin:
+// 1) 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.
+// 2) 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" found in the firmware).
+// 3) 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, the following is an ASCII art of this convention.
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -161,7 +169,7 @@
//
//
//
-// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+// This function WILL NEED TO BE edited for successful completion of the classroom exercise
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
@@ -459,7 +467,7 @@ void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request
response.controlOutput.motorCmd2 = 0;
response.controlOutput.motorCmd3 = 0;
response.controlOutput.motorCmd4 = 0;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
break;
}
}
@@ -542,7 +550,7 @@ void calculateControlOutput_viaLQRforMotors(float stateErrorBody[12], Controller
// Specify that this controller is a rate controller
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -633,7 +641,7 @@ void calculateControlOutput_viaLQRforActuators(float stateErrorBody[12], Control
// Specify that this controller is a rate controller
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -730,7 +738,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -816,7 +824,7 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -921,7 +929,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -1055,7 +1063,7 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Cont
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -1251,7 +1259,7 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
// > 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
+ // for successful completion of the classroom exercise
convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
}
@@ -1272,7 +1280,7 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
// 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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
float computeMotorPolyBackward(float thrust)
{
// Compute the 16-bit command signal that generates the "thrust" force
@@ -1308,7 +1316,7 @@ float computeMotorPolyBackward(float thrust)
// 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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
void setpointCallback(const Setpoint& newSetpoint)
{
setpoint[0] = newSetpoint.x;
@@ -1319,7 +1327,7 @@ void setpointCallback(const Setpoint& newSetpoint)
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom 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)
@@ -1443,94 +1451,97 @@ void publish_current_xyz_yaw(float x, float y, float z, float yaw)
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+// This function DOES NOT NEED TO BE edited for successful completion
+// ofthe exercise
+void isReadyDemoControllerYamlCallback(const IntWithHeader & msg)
{
- // Extract from the "msg" for which controller the and from where to fetch the YAML
- // parameters
- int controller_to_fetch_yaml = msg.data;
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
- // Switch between fetching for the different controllers and from different locations
- switch(controller_to_fetch_yaml)
+ // Continue if the message is relevant
+ if (isRevelant)
{
-
- // > 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:
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
{
- // 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;
- }
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[DEMO CONTROLLER] Now fetching the DemoController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[DEMO CONTROLLER] Now fetching the DemoController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_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;
+ default:
+ {
+ ROS_ERROR("[STUDENT CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
}
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchDemoControllerYamlParameters(nodeHandle_to_use);
}
}
-// This function CAN BE edited for successful completion of the PPS exercise, and the
+
+// This function CAN BE edited for successful completion of the classroom 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)
+void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle)
{
- // Here we load the parameters that are specified in the DemoController.yaml file
+ // Here we load the parameters that are specified in the file:
+ // DemoController.yaml file
// Add the "DemoController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_demoController(nodeHandle, "DemoController");
+ ros::NodeHandle nodeHandle_for_paraMaters(nodeHandle, "DemoController");
// > The mass of the crazyflie
- cf_mass = getParameterFloat(nodeHandle_for_demoController , "mass");
+ cf_mass = getParameterFloat(nodeHandle_for_paraMaters , "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");
+ vicon_frequency = getParameterFloat(nodeHandle_for_paraMaters, "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");
+ control_frequency = getParameterFloat(nodeHandle_for_paraMaters, "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);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
- shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_demoController, "shouldPerformConvertIntoBodyFrame");
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paraMaters, "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");
+ shouldPublishCurrent_xyz_yaw = getParameterBool(nodeHandle_for_paraMaters, "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");
+ shouldFollowAnotherAgent = getParameterBool(nodeHandle_for_paraMaters, "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);
+ int temp_number_of_agents_in_a_line = getParameterIntVectorWithUnknownLength(nodeHandle_for_paraMaters, "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() )
{
@@ -1539,70 +1550,70 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
}
// Boolean indiciating whether the "Debug Message" of this agent should be published or not
- shouldPublishDebugMessage = getParameterBool(nodeHandle_for_demoController, "shouldPublishDebugMessage");
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paraMaters, "shouldPublishDebugMessage");
// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
- shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_demoController, "shouldDisplayDebugInfo");
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paraMaters, "shouldDisplayDebugInfo");
// THE CONTROLLER GAINS:
// A flag for which controller to use:
- controller_mode = getParameterInt( nodeHandle_for_demoController , "controller_mode" );
+ controller_mode = getParameterInt( nodeHandle_for_paraMaters , "controller_mode" );
// A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_demoController , "estimator_method" );
+ estimator_method = getParameterInt( nodeHandle_for_paraMaters , "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);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor1", gainMatrixMotor1, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor2", gainMatrixMotor2, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor3", gainMatrixMotor3, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "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);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_TwelveStateVector", gainMatrixThrust_TwelveStateVector, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollTorque", gainMatrixRollTorque, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchTorque", gainMatrixPitchTorque, 12);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "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);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "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);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "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_paraMaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "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);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "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");
+ angleResponseTest_pitchAngle_degrees = getParameterFloat(nodeHandle_for_paraMaters, "angleResponseTest_pitchAngle_degrees");
+ angleResponseTest_distance_meters = getParameterFloat(nodeHandle_for_paraMaters, "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");
+ cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_paraMaters, "command_sixteenbit_min");
+ cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_paraMaters, "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);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "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);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
@@ -1616,7 +1627,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
}
-// This function CAN BE edited for successful completion of the PPS exercise, and the
+// This function CAN BE edited for successful completion of the classroom 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()
@@ -1646,7 +1657,7 @@ void processFetchedParameters()
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)
+ if (follow_in_a_line_agentIDs[i] == m_agentID)
{
// Set the boolean flag that we have found out own agent ID
foundMyAgentID = true;
@@ -1681,109 +1692,38 @@ void processFetchedParameters()
// 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
+ //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 );
- }
+ if (shouldFollowAnotherAgent)
+ {
+ ROS_INFO_STREAM("[DEMO CONTROLLER] This agent (with ID " << m_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;
-}
-
-
-
-
// ----------------------------------------------------------------------------------
@@ -1794,145 +1734,183 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
// M M A A III N N
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom 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");
+
+ // 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);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "studentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[DEMO CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[DEMO CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
}
- // *********************************************************************************
- // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
- // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
- // 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";
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
- // 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);
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[DEMO CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[DEMO CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_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);
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
- // 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";
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber demoContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "DemoController", 1, isReadyDemoControllerYamlCallback);
+ ros::Subscriber demoContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("DemoController", 1, isReadyDemoControllerYamlCallback);
- // 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);
+ // GIVE YAML VARIABLES AN INITIAL VALUE
- // PRINT OUT SOME INFORMATION
+ // This can be done either here or as part of declaring the variable
+ // in the header file
+ //yaml_cf_mass_in_grams = 25.0;
- // 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"
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to "Parametr
+ // Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // message containing the filename of the *.yaml file, and
+ // then a message will be received on the above subscribers
+ // when the paramters are ready.
+
+ // Create a publisher for request the yaml load
+ // > created as a local variable
+ // > note importantly that the final argument is "true", this
+ // enables "latching" on the connection. When a connection is
+ // latched, the last message published is saved and
+ // automatically sent to any future subscribers that connect.
+ ros::Publisher requestLoadYamlFilenamePublisher = nodeHandle_to_own_agent_parameter_service.advertise<StringWithHeader>("requestLoadYamlFilename", 1, true);
+ // Create a local variable for the message
+ StringWithHeader yaml_filename_msg;
+ // Specify the data
+ yaml_filename_msg.data = "DemoController";
+ // Set for whom this applies to
+ yaml_filename_msg.shouldCheckForAgentID = false;
+ // Sleep to make the publisher known to ROS (in seconds)
+ //ros::Duration(1.0).sleep();
+ // Send the message
+ requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+
+
// 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;
+ //fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+
+
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable 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., "dfall_pkg" as specified by the line:
+ // "using namespace dfall_pkg;"
+ // 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 "<m_agentID>/my_current_xyz_yaw_topic" where <m_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/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..14c80a6a76b2ecf030101b6f77653eb06d3aadee
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -0,0 +1,1552 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+// 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.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/FlyingAgentClient.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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M OOO TTTTT III OOO N N
+// MM MM O O T I O O NN N
+// M M M O O T I O O N N N
+// M M O O T I O O N NN
+// M M OOO T III OOO N N
+//
+// CCCC A PPPP TTTTT U U RRRR EEEEE
+// C A A P P T U U R R E
+// C A A PPPP T U U RRRR EEE
+// C AAAAA P T U U R R E
+// CCCC A A P T UUU R R EEEEE
+// ----------------------------------------------------------------------------------
+
+// CALLBACK RUN EVERY TIME NEW MOTION CAPTURE DATA IS RECEIVED
+void viconCallback(const ViconData& viconData)
+{
+ // NOTE: THIS FUNCTION IS CALL AT THE FREQUENCY OF THE MOTION
+ // CAPTURE SYSTEM. HENCE ANY OPTERATIONS PERFORMED IN
+ // THIS FUNCTION MUST BE NON-BLOCKING.
+
+ // Initialise a counter of consecutive frames of motion
+ // capture data where the agent is occuled
+ static int number_of_consecutive_occulsions = 0;
+
+ // Initilise a variable for the pose data of this agent
+ CrazyflieData poseDataForThisAgent;
+
+ // Extract the pose data from the full motion capture array
+ // NOTE: that if the return index is a negative then this
+ // indicates that the pose data was not found.
+ m_poseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, m_context.crazyflieName , m_poseDataIndex , poseDataForThisAgent );
+
+
+ // Detecting time-out of the motion capture data
+ // > Update the flag
+ m_isAvailable_mocap_data = true;
+ // > Stop any previous instance that might still be running
+ m_timer_mocap_timeout_check.stop();
+ // > Set the period again (second argument is reset)
+ m_timer_mocap_timeout_check.setPeriod( ros::Duration(yaml_mocap_timeout_duration), true);
+ // > Start the timer again
+ m_timer_mocap_timeout_check.start();
+
+
+ // Only continue if:
+ // (1) the pose for this agent was found, and
+ // (2) the controllers are actually available
+ // (2) the flying state is something other than MOTORS-OFF
+ if (
+ (m_poseDataIndex >= 0)
+ and
+ (m_controllers_avialble)
+ )
+ {
+
+ // Only continue if:
+ // (1) the agent is NOT occulded
+ if(!poseDataForThisAgent.occluded)
+ {
+ // Update the flag
+ m_isOcculded_mocap_data = false;
+ // Reset the "consecutive occulsions" flag
+ number_of_consecutive_occulsions = 0;
+
+
+ // Only continue if:
+ // (1) The state is different from MOTORS-OFF
+ if (m_flying_state != STATE_MOTORS_OFF)
+ {
+
+ // Initliase the "Contrller" service call variable
+ Controller controllerCall;
+
+ // Fill in the pose data for this agent
+ controllerCall.request.ownCrazyflie = poseDataForThisAgent;
+
+
+
+
+ // PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
+ if ( getInstantController() != DEFAULT_CONTROLLER )
+ {
+ if ( !safetyCheck_on_positionAndTilt(poseDataForThisAgent) )
+ {
+ setInstantController(DEFAULT_CONTROLLER);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] safety check failed, switching to DEFAULT CONTROLLER");
+ }
+ }
+
+ // Initialise a local boolean success variable
+ bool isSuccessful_controllerCall = false;
+
+ // Call the controller service client
+ isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
+
+ // Ensure success and enforce safety
+ if(!isSuccessful_controllerCall)
+ {
+ // Let the user know that the controller call failed
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
+
+ // Switch to the default controller,
+ // if it was not already the active controller
+ if ( getInstantController() != DEFAULT_CONTROLLER )
+ {
+ // Set the DEFAULT controller to be active
+ setInstantController(DEFAULT_CONTROLLER);
+ // Try the controller call
+ isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
+ // Inform the user is not successful
+ if ( !isSuccessful_controllerCall )
+ {
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Also failed to call the DEFAULT controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
+ }
+ }
+ }
+
+ // Send the command to the CrazyRadio
+ // > IF SUCCESSFUL
+ if (isSuccessful_controllerCall)
+ {
+ m_commandForSendingToCrazyfliePublisher.publish(controllerCall.response.controlOutput);
+ }
+ // > ELSE SEND ZERO OUTPUT COMMAND
+ else
+ {
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+ // And change the state to motor-off
+ requestChangeFlyingStateTo(STATE_MOTORS_OFF);
+ }
+ }
+ else
+ {
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+ } // END OF: "if (m_flying_state != STATE_MOTORS_OFF)"
+ }
+ else
+ {
+ // Increment the number of consective occulations
+ number_of_consecutive_occulsions++;
+ // Update the flag if this exceeds the threshold
+ if (
+ (!m_isOcculded_mocap_data)
+ and
+ (number_of_consecutive_occulsions > yaml_consecutive_occulsions_threshold)
+ )
+ {
+ // Update the flag
+ m_isOcculded_mocap_data = true;
+ // Inform the user
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Agent was occluded for more than " << yaml_consecutive_occulsions_threshold << " consecutive frames." );
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+ // Update the flying state
+ requestChangeFlyingStateTo( STATE_MOTORS_OFF );
+ }
+ } // END OF: "if(!poseDataForThisAgent.occluded)"
+
+ }
+ else
+ {
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+
+ } // END OF: "if ( (m_poseDataIndex >= 0) and (m_controllers_avialble) )"
+
+}
+
+
+
+
+
+int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index , CrazyflieData& pose_to_fill_in)
+{
+ // Initialise an integer for the index where the object
+ // "name" is found
+ // > Initialise an negative to indicate not found
+ int object_index = -1;
+
+ // Get the length of the "pose data array"
+ int length_poseData = viconData.crazyflies.size();
+
+ // If the "expected index" is non-negative and less than
+ // the length of the data array, then attempt to check
+ // for a name match
+ if (
+ (0 <= expected_index)
+ and
+ (expected_index < length_poseData)
+ )
+ {
+ // Check if the names match
+ if (viconData.crazyflies[expected_index].crazyflieName == m_context.crazyflieName)
+ {
+ object_index = expected_index;
+ }
+ }
+
+ // If not found, then iterate the data array looking
+ // for a name match
+ if (object_index < 0)
+ {
+ for( int i=0 ; i<length_poseData ; i++ )
+ {
+ // Check if the names match
+ if(viconData.crazyflies[i].crazyflieName == m_context.crazyflieName)
+ {
+ object_index = i;
+ }
+ }
+ }
+
+ // If not found, then retrun, else fill in the pose data
+ if (object_index < 0)
+ {
+ return object_index;
+ }
+ else
+ {
+ pose_to_fill_in = viconData.crazyflies[object_index];
+ coordinatesToLocal(pose_to_fill_in);
+ return object_index;
+ }
+}
+
+
+void coordinatesToLocal(CrazyflieData& cf)
+{
+ // Get the area into a local variable
+ AreaBounds area = m_context.localArea;
+
+ // Shift the X-Y coordinates
+ float originX = (area.xmin + area.xmax) / 2.0;
+ float originY = (area.ymin + area.ymax) / 2.0;
+ cf.x -= originX;
+ cf.y -= originY;
+
+ // Shift the Z coordinate
+ // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
+ //float originZ = 0.0;
+ // float originZ = (area.zmin + area.zmax) / 2.0;
+ //cf.z -= originZ;
+}
+
+
+void timerCallback_mocap_timeout_check(const ros::TimerEvent&)
+{
+ // Update the flag
+ m_isAvailable_mocap_data = false;
+ // Inform the user
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Motion Capture Data has been unavailable for " << yaml_mocap_timeout_duration << " seconds." );
+ // Ensure that the motors are turned off
+ if ( !(m_flying_state==STATE_MOTORS_OFF) )
+ {
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+ // Update the flying state
+ requestChangeFlyingStateTo( STATE_MOTORS_OFF );
+ }
+}
+
+
+void sendZeroOutputCommandForMotors()
+{
+ ControlCommand zeroOutput = ControlCommand(); //everything set to zero
+ zeroOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS; //set to motor_mode
+ m_commandForSendingToCrazyfliePublisher.publish(zeroOutput);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS A FFFFF EEEEE TTTTT Y Y
+// S A A F E T Y Y
+// SSS A A FFF EEE T Y
+// S AAAAA F E T Y
+// SSSS A A F EEEEE T Y
+//
+// CCCC H H EEEEE CCCC K K SSSS
+// C H H E C K K S
+// C HHHHH EEE C KKK SSS
+// C H H E C K K S
+// CCCC H H EEEEE CCCC K K SSSS
+// ----------------------------------------------------------------------------------
+
+// Checks if crazyflie is within allowed area
+bool safetyCheck_on_positionAndTilt(CrazyflieData data)
+{
+ // Check on the X position
+ if((data.x < m_context.localArea.xmin) or (data.x > m_context.localArea.xmax))
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] x safety failed");
+ return false;
+ }
+ // Check on the Y position
+ if((data.y < m_context.localArea.ymin) or (data.y > m_context.localArea.ymax))
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] y safety failed");
+ return false;
+ }
+ // Check on the Z position
+ if((data.z < m_context.localArea.zmin) or (data.z > m_context.localArea.zmax))
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] z safety failed");
+ return false;
+ }
+
+ // Check the title angle (if required)
+ // > The tilt anlge between the body frame and inertial frame z-axis is
+ // give by:
+ // tilt angle = 1 / ( cos(roll)*cos(pitch) )
+ // > But this would be too sensitve to a divide by zero error, so instead
+ // we just check if each angle separately exceeds the limit
+ if(yaml_isEnabled_strictSafety)
+ {
+ // Check on the ROLL angle
+ if(
+ (data.roll > yaml_maxTiltAngle_for_strictSafety_radians)
+ or
+ (data.roll < -yaml_maxTiltAngle_for_strictSafety_radians)
+ )
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] roll too big.");
+ return false;
+ }
+ // Check on the PITCH angle
+ if(
+ (data.pitch > yaml_maxTiltAngle_for_strictSafety_radians)
+ or
+ (data.pitch < -yaml_maxTiltAngle_for_strictSafety_radians)
+ )
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] pitch too big.");
+ return false;
+ }
+ }
+
+ // If the code makes it to here then all the safety checks passed,
+ // Hence return "true"
+ return true;
+}
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF L Y Y III N N GGGG
+// F L Y Y I NN N G
+// FFF L Y I N N N G
+// F L Y I N NN G G
+// F LLLLL Y III N N GGGG
+//
+// SSSS TTTTT A TTTTT EEEEE
+// S T A A T E
+// SSS T A A T EEE
+// S T AAAAA T E
+// SSSS T A A T EEEEE
+// ----------------------------------------------------------------------------------
+
+
+void requestChangeFlyingStateTo(int new_state)
+{
+ if(m_crazyradio_status != CRAZY_RADIO_STATE_CONNECTED)
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] Disconnected and trying to change state. State goes to MOTORS OFF");
+ m_flying_state = STATE_MOTORS_OFF;
+ }
+ else
+ {
+ // Switch between the possible new states
+ switch (new_state)
+ {
+ case STATE_TAKE_OFF:
+ {
+ requestChangeFlyingStateToTakeOff();
+ break;
+ }
+
+ case STATE_FLYING:
+ {
+ // This should never be called
+ break;
+ }
+
+ case STATE_LAND:
+ {
+ requestChangeFlyingStateToLand();
+ break;
+ }
+
+ case STATE_MOTORS_OFF:
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] Change state to MOTORS OFF");
+ m_flying_state = new_state;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Request state of " << new_state << " not recognised. Hence changing to MOTORS OFF.");
+ m_flying_state = new_state;
+ break;
+ }
+ }
+
+ }
+
+ // Publish a message with the new flying state
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ m_flyingStatePublisher.publish(flying_state_msg);
+}
+
+void requestChangeFlyingStateToTakeOff()
+{
+ // Only allow taking off from the MOTORS OFF state
+ if (m_flying_state != STATE_MOTORS_OFF)
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Request to TAKE OFF was not implemented because the current state is NOT the MOTORS OFF state.");
+ }
+ else
+ {
+ // Check that the Motion Capture data is available
+ if ( m_isAvailable_mocap_data and !m_isOcculded_mocap_data )
+ {
+ // Check whether a "controller" take-off should
+ // be performed, and that not already in the
+ // "take-off" state
+ if (
+ (yaml_shouldPerfom_takeOff_with_defaultController)
+ and
+ (m_flying_state != STATE_TAKE_OFF)
+ )
+ {
+ // Call the service offered by the default
+ // controller for how long a take-off will take
+ dfall_pkg::IntIntService requestManoeurveCall;
+ requestManoeurveCall.request.data = DEFAULT_CONTROLLER_REQUEST_TAKEOFF;
+ if(m_defaultController_requestManoeuvre.call(requestManoeurveCall))
+ {
+ // Extract the duration
+ float take_off_duration = float(requestManoeurveCall.response.data) / 1000.0;
+ // Start the timer
+ // > Stop any previous instance
+ m_timer_takeoff_complete.stop();
+ // > Set the period again (second argument is reset)
+ m_timer_takeoff_complete.setPeriod( ros::Duration(take_off_duration), true);
+ // > Start the timer
+ m_timer_takeoff_complete.start();
+ // Inform the user
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] Changed state to STATE_TAKE_OFF for a duration of " << take_off_duration << " seconds.");
+ // Update the class variable
+ m_flying_state = STATE_TAKE_OFF;
+ // Set the Default controller as the instant controller
+ setInstantController(DEFAULT_CONTROLLER);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Failed to get take-off duration from Default controller. Switching to MOTORS-OFF.");
+ // Update the class variable
+ m_flying_state = STATE_MOTORS_OFF;
+ }
+ }
+ // Otherwise, just switch straight to the
+ // "flying" state
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Changed state directly to STATE_FLYING");
+ // Update the class variable
+ m_flying_state = STATE_FLYING;
+ }
+ }
+ else
+ {
+ // Inform the user of the problem
+ if (!m_isAvailable_mocap_data)
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the motion capture data is unavailable.");
+ }
+ if (m_isOcculded_mocap_data)
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the object is \"long-term\" occuled.");
+ }
+ }
+ }
+}
+
+
+void requestChangeFlyingStateToLand()
+{
+ // Only allow landing from the TAKE-OFF and FLYING state
+ if (
+ (m_flying_state != STATE_TAKE_OFF)
+ and
+ (m_flying_state != STATE_FLYING)
+ )
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Request to LAND was not implemented because the current state is NOT the TAKE-OFF or FLYING state.");
+ }
+ else
+ {
+ // Check whether a "controller" take-off should
+ // be performed, and that not already in the
+ // "land" state
+ if (
+ (yaml_shouldPerfom_landing_with_defaultController)
+ and
+ (m_flying_state != STATE_LAND)
+ )
+ {
+ // Call the service offered by the default
+ // controller for how long a landing will take
+ dfall_pkg::IntIntService requestManoeurveCall;
+ requestManoeurveCall.request.data = DEFAULT_CONTROLLER_REQUEST_LANDING;
+ if(m_defaultController_requestManoeuvre.call(requestManoeurveCall))
+ {
+ // Extract the duration
+ float land_duration = float(requestManoeurveCall.response.data) / 1000.0;
+ // Start the timer
+ // > Stop any previous instance
+ m_timer_land_complete.stop();
+ // > Set the period again (second argument is reset)
+ m_timer_land_complete.setPeriod( ros::Duration(land_duration), true);
+ // > Start the timer
+ m_timer_land_complete.start();
+ // Inform the user
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] Changed state to STATE_LAND for a duration of " << land_duration << " seconds.");
+ // Update the class variable
+ m_flying_state = STATE_LAND;
+ // Set the Default controller as the instant controller
+ setInstantController(DEFAULT_CONTROLLER);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Failed to get land duration from Default controller. Switching to MOTORS-OFF.");
+ // Update the class variable
+ m_flying_state = STATE_MOTORS_OFF;
+ }
+ }
+ // Otherwise, just switch straight to the
+ // "motors off" state
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Changed state directly to STATE_MOTORS_OFF");
+ // Update the class variable
+ m_flying_state = STATE_MOTORS_OFF;
+ }
+ }
+}
+
+
+void timerCallback_takeoff_complete(const ros::TimerEvent&)
+{
+ // Only change to flying if still in the take-off state
+ if (m_flying_state == STATE_TAKE_OFF)
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Take-off complete, changed state to STATE_FLYING");
+ // Update the class variable
+ m_flying_state = STATE_FLYING;
+ // Publish a message with the new flying state
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ m_flyingStatePublisher.publish(flying_state_msg);
+ // Change back to the nominal controller
+ setInstantController( m_controller_nominally_selected );
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Take-off duration elapsed but the agent is no longer in STATE_TAKE_OFF.");
+ }
+}
+
+void timerCallback_land_complete(const ros::TimerEvent&)
+{
+ // Only change to flying if still in the take-off state
+ if (m_flying_state == STATE_LAND)
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Land complete, changed state to STATE_MOTORS_OFF");
+ // Update the class variable
+ m_flying_state = STATE_MOTORS_OFF;
+ // Publish a message with the new flying state
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ m_flyingStatePublisher.publish(flying_state_msg);
+ // Change back to the nominal controller
+ setInstantController( m_controller_nominally_selected );
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Land duration elapsed but the agent is no longer in STATE_LAND.");
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE L EEEEE CCCC TTTTT
+// S E L E C T
+// SSS EEE L EEE C T
+// S E L E C T
+// SSSS EEEEE LLLLL EEEEE CCCC T
+//
+// CCCC OOO N N TTTTT RRRR OOO L L EEEEE RRRR
+// C O O NN N T R R O O L L E R R
+// C O O N N N T RRRR O O L L EEE RRRR
+// C O O N NN T R R O O L L E R R
+// CCCC OOO N N T R R OOO LLLLL LLLLL EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+// THIS SETS THE CONTROLLER THAT IT ACTUALLY BEING USED
+// > During take-off and landing this function is
+// called to switch to the "Default" controller
+// > The highlighting in the "Flying Agent GUI" should
+// reflect the instant controller
+void setInstantController(int controller)
+{
+ // Update the class variable
+ m_instant_controller = controller;
+
+ // Point to the correct service client
+ switch(controller)
+ {
+ case DEFAULT_CONTROLLER:
+ m_instant_controller_service_client = &m_defaultController;
+ break;
+ case STUDENT_CONTROLLER:
+ m_instant_controller_service_client = &m_studentController;
+ break;
+ case TUNING_CONTROLLER:
+ m_instant_controller_service_client = &m_tuningController;
+ break;
+ case PICKER_CONTROLLER:
+ m_instant_controller_service_client = &m_pickerController;
+ break;
+ case TEMPLATE_CONTROLLER:
+ m_instant_controller_service_client = &m_templateController;
+ break;
+ default:
+ break;
+ }
+
+ // Publish a message that informs the "Flying Agent
+ // GUI" about this update to the instant controller
+ std_msgs::Int32 controller_used_msg;
+ controller_used_msg.data = controller;
+ m_controllerUsedPublisher.publish(controller_used_msg);
+}
+
+
+// THIS SIMPLY RETRIEVES THE CLASS VARIABLE
+int getInstantController()
+{
+ return m_instant_controller;
+}
+
+
+// THIS SETS THE CONTROLLER THAT IS NOMINALLY SELECTED
+// > This is the controller that will be use as the
+// "instant controller" when in the "flying" state.
+// > But during take-off and landing, the "Default"
+// controller is used, and this keeps track of which
+// controller to switch to after those phases are
+// complete
+void setControllerNominallySelected(int controller)
+{
+ // Update the class variable
+ m_controller_nominally_selected = controller;
+
+ // If in state "MOTORS-OFF" or "FLYING",
+ // then the change is instant.
+ if (
+ (m_flying_state == STATE_MOTORS_OFF)
+ or
+ (m_flying_state == STATE_FLYING)
+ )
+ {
+
+ setInstantController(controller);
+ }
+}
+
+
+// THIS SIMPLY RETRIEVES THE CLASS VARIABLE
+int getControllerNominallySelected()
+{
+ return m_controller_nominally_selected;
+}
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS TTTTT A TTTTT EEEEE
+// S T A A T E
+// SSS T A A T EEE
+// S T AAAAA T E
+// SSSS T A A T EEEEE
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+//
+// FFFFF RRRR OOO M M GGGG U U III
+// F R R O O MM MM G U U I
+// FFF RRRR O O M M M G U U I
+// F R R O O M M G G U U I
+// F R R OOO M M GGGG UUU III
+// ----------------------------------------------------------------------------------
+
+
+
+// THE CALLBACK THAT A NEW FLYING STATE WAS REQUESTED
+// > These requests come from the "Flying Agent GUI"
+// > The options are: {take-off,land,motor-off,controller}
+void flyingStateRequestCallback(const IntWithHeader & msg) {
+
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int cmd = msg.data;
+
+ switch(cmd) {
+ case CMD_USE_DEFAULT_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_DEFAULT_CONTROLLER Command received");
+ setControllerNominallySelected(DEFAULT_CONTROLLER);
+ break;
+
+ case CMD_USE_DEMO_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_DEMO_CONTROLLER Command received");
+ setControllerNominallySelected(DEMO_CONTROLLER);
+ break;
+
+ case CMD_USE_STUDENT_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_STUDENT_CONTROLLER Command received");
+ setControllerNominallySelected(STUDENT_CONTROLLER);
+ break;
+
+ case CMD_USE_MPC_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_MPC_CONTROLLER Command received");
+ setControllerNominallySelected(MPC_CONTROLLER);
+ break;
+
+ case CMD_USE_REMOTE_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_REMOTE_CONTROLLER Command received");
+ setControllerNominallySelected(REMOTE_CONTROLLER);
+ break;
+
+ case CMD_USE_TUNING_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_TUNING_CONTROLLER Command received");
+ setControllerNominallySelected(TUNING_CONTROLLER);
+ break;
+
+ case CMD_USE_PICKER_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_PICKER_CONTROLLER Command received");
+ setControllerNominallySelected(PICKER_CONTROLLER);
+ break;
+
+ case CMD_USE_TEMPLATE_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_TEMPLATE_CONTROLLER Command received");
+ setControllerNominallySelected(TEMPLATE_CONTROLLER);
+ break;
+
+ case CMD_CRAZYFLY_TAKE_OFF:
+ ROS_INFO("[FLYING AGENT CLIENT] TAKE_OFF Command received");
+ requestChangeFlyingStateTo(STATE_TAKE_OFF);
+ break;
+
+ case CMD_CRAZYFLY_LAND:
+ ROS_INFO("[FLYING AGENT CLIENT] LAND Command received");
+ requestChangeFlyingStateTo(STATE_LAND);
+ break;
+ case CMD_CRAZYFLY_MOTORS_OFF:
+ ROS_INFO("[FLYING AGENT CLIENT] MOTORS_OFF Command received");
+ requestChangeFlyingStateTo(STATE_MOTORS_OFF);
+ break;
+
+ default:
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] unexpected command number: " << cmd);
+ break;
+ }
+ }
+}
+
+
+
+
+
+
+
+
+void crazyRadioStatusCallback(const std_msgs::Int32& msg)
+{
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] Received message with Crazy Radio Status = " << msg.data );
+ m_crazyradio_status = msg.data;
+}
+
+
+
+
+
+
+void emergencyStopReceivedCallback(const IntWithHeader & msg)
+{
+ ROS_INFO("[FLYING AGENT CLIENT] Received message to EMERGENCY STOP");
+ flyingStateRequestCallback(msg);
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// GGGG EEEEE TTTTT SSSS TTTTT A TTTTT U U SSSS
+// G E T S T A A T U U S
+// G EEE T SSS T A A T U U SSS
+// G G E T S T AAAAA T U U S
+// GGGG EEEEE T SSSS T A A T UUU SSSS
+//
+// CCCC A L L BBBB A CCCC K K SSSS
+// C A A L L B B A A C K K S
+// C A A L L BBBB A A C KKK SSS
+// C AAAAA L L B B AAAAA C K K S
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K SSSS
+// ----------------------------------------------------------------------------------
+
+bool getCurrentFlyingStateServiceCallback(IntIntService::Request &request, IntIntService::Response &response)
+{
+ // Put the flying state into the response variable
+ response.data = m_flying_state;
+ // Return
+ return true;
+}
+
+
+bool getInstantControllerServiceCallback(IntIntService::Request &request, IntIntService::Response &response)
+{
+ // Put the flying state into the response variable
+ response.data = getInstantController();
+ // Return
+ return true;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
+// ----------------------------------------------------------------------------------
+
+void batteryMonitorStateChangedCallback(std_msgs::Int32 msg)
+{
+ // Extract the data
+ int new_battery_state = msg.data;
+
+ // Take action if changed to low battery state
+ if (new_battery_state == BATTERY_STATE_LOW)
+ {
+ if (m_flying_state != STATE_MOTORS_OFF)
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] low level battery triggered, now landing.");
+ requestChangeFlyingStateTo(STATE_LAND);
+ }
+ else
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] low level battery triggered, please turn off the Crazyflie.");
+ }
+ }
+ else if (new_battery_state == BATTERY_STATE_NORMAL)
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] received message that battery state changed to normal");
+ }
+ else
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] received message that battery state changed to something unknown");
+ }
+
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+//
+// CCCC OOO N N TTTTT EEEEE X X TTTTT
+// C O O NN N T E X X T
+// C O O N N N T EEE X T
+// C O O N NN T E X X T
+// CCCC OOO N N T EEEEE X X T
+// ----------------------------------------------------------------------------------
+
+
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg)
+{
+ ROS_INFO("[FLYING AGENT CLIENT] Received message that the Context Database Changed");
+ loadCrazyflieContext();
+}
+
+
+
+void loadCrazyflieContext()
+{
+ CMQuery contextCall;
+ contextCall.request.studentID = m_agentID;
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] AgentID:" << m_agentID);
+
+ CrazyflieContext new_context;
+
+ m_centralManager.waitForExistence(ros::Duration(-1));
+
+ if(m_centralManager.call(contextCall)) {
+ new_context = contextCall.response.crazyflieContext;
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] CrazyflieContext:\n" << new_context);
+
+ if((m_context.crazyflieName != "") && (new_context.crazyflieName != m_context.crazyflieName)) //linked crazyflie name changed and it was not empty before
+ {
+
+ // Motors off is done in python script now everytime we disconnect
+
+ // send motors OFF and disconnect before setting m_context = new_context
+ // std_msgs::Int32 msg;
+ // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ // commandPublisher.publish(msg);
+
+ ROS_INFO("[FLYING AGENT CLIENT] CF is now different for this student. Disconnect and turn it off");
+
+ IntWithHeader msg;
+ msg.shouldCheckForAgentID = false;
+ msg.data = CMD_DISCONNECT;
+ m_crazyRadioCommandPublisher.publish(msg);
+ }
+
+ m_context = new_context;
+
+ ros::NodeHandle nh("CrazyRadio");
+ nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
+ }
+ else
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Failed to load context. Waiting for next Save in DB by teacher");
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC RRRR EEEEE A TTTTT EEEEE
+// C R R E A A T E
+// C RRRR EEE A A T EEE
+// C R R E AAAAA T E
+// CCCC R R EEEEE A A T EEEEE
+//
+// SSSS EEEEE RRRR V V III CCCC EEEEE
+// S E R R V V I C E
+// SSS EEE RRRR V V I C EEE
+// S E R R V V I C E
+// SSSS EEEEE R R V III CCCC EEEEE
+//
+// CCCC L III EEEEE N N TTTTT
+// C L I E NN N T
+// C L I EEE N N N T
+// C L I E N NN T
+// CCCC LLLLL III EEEEE N N T
+// ----------------------------------------------------------------------------------
+
+
+// CREATE A "CONTROLLER" TYPE SERVICE CLIENT
+// NOTE: that in the "ros::service::createClient" function the
+// second argument is a boolean that specifies whether the
+// service is persistent or not. In the ROS documentation a
+// persistent connection is described as:
+// "Persistent connections should be used carefully. They greatly
+// improve performance for repeated requests, but they also make
+// your client more fragile to service failures. Clients using
+// persistent connections should implement their own reconnection
+// logic in the event that the persistent connection fails."
+void createControllerServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient )
+{
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "FlyingAgentClientConfig");
+
+ std::string controllerName;
+ if(!nodeHandle.getParam(paramter_name, controllerName))
+ {
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to get \"" << paramter_name << "\" paramter");
+ return;
+ }
+
+ serviceClient = ros::service::createClient<Controller>(controllerName, true);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded service: " << serviceClient.getService() << ", valid: " << serviceClient.isValid() << ", exists: " << serviceClient.exists() );
+}
+
+
+void createIntIntServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient )
+{
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "FlyingAgentClientConfig");
+
+ std::string controllerName;
+ if(!nodeHandle.getParam(paramter_name, controllerName))
+ {
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to get \"" << paramter_name << "\" paramter");
+ return;
+ }
+
+ serviceClient = ros::service::createClient<IntIntService>(controllerName, true);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded service: " << serviceClient.getService() << ", valid: " << serviceClient.isValid() << ", exists: " << serviceClient.exists() );
+}
+
+
+
+void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
+{
+ // INITIALISE ALL THE CONTROLLER SERVICE CLIENTS
+ createControllerServiceClientFromParameterName( "defaultController" , m_defaultController );
+ createControllerServiceClientFromParameterName( "studentController" , m_studentController );
+ createControllerServiceClientFromParameterName( "tuningController" , m_tuningController );
+ createControllerServiceClientFromParameterName( "pickerController" , m_pickerController );
+ createControllerServiceClientFromParameterName( "templateController" , m_templateController );
+
+ // INITIALISE THE SERVICE FOR REQUESTING THE DEFAULT
+ // CONTROLLER TO PERFORM MANOEUVRES
+ createIntIntServiceClientFromParameterName( "defaultController_requestManoeuvre" , m_defaultController_requestManoeuvre );
+
+ // Check that at least the default controller is available
+ // > Setting the flag accordingly
+ if (m_defaultController)
+ {
+ m_controllers_avialble = true;
+ }
+ else
+ {
+ m_controllers_avialble = false;
+ // Inform the user of the problem
+ ROS_ERROR("[FLYING AGENT CLIENT] The default controller service client (and pressumably all other controllers) could NOT be created.");
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+void isReadyFlyingAgentClientConfigYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] Now fetching the FlyingAgentClientConfig YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] Now fetching the FlyingAgentClientConfig YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchFlyingAgentClientConfigYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+
+// > Load the paramters from the Client Config YAML file
+void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // FlyingAgentClientConfig.yaml
+
+ // Add the "FlyingAgentClientConfig" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "FlyingAgentClientConfig");
+
+ // Flag for whether to use angle for switching to the Safe Controller
+ yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
+ yaml_maxTiltAngle_for_strictSafety_degrees = getParameterFloat(nodeHandle_for_paramaters,"maxTiltAngle_for_strictSafety_degrees");
+
+ // Number of consecutive occulsions that will deem the
+ // object as "long-term" occuled
+ yaml_consecutive_occulsions_threshold = getParameterInt(nodeHandle_for_paramaters,"consecutive_occulsions_threshold");
+
+ // Time out duration after which Mocap is considered unavailable
+ yaml_mocap_timeout_duration = getParameterFloat(nodeHandle_for_paramaters,"mocap_timeout_duration");
+
+ // Flag for whether the take-off should be performed
+ // with the default controller
+ yaml_shouldPerfom_takeOff_with_defaultController = getParameterBool(nodeHandle_for_paramaters,"shouldPerfom_takeOff_with_defaultController");
+
+ // Flag for whether the landing should be performed
+ // with the default controller
+ yaml_shouldPerfom_landing_with_defaultController = getParameterBool(nodeHandle_for_paramaters,"shouldPerfom_landing_with_defaultController");
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched FlyingAgentClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
+
+
+
+ // PROCESS THE PARAMTERS
+ // Convert from degrees to radians
+ yaml_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
+
+ // DEBUGGING: Print out one of the processed values
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing yaml_maxTiltAngle_for_strictSafety_radians = " << yaml_maxTiltAngle_for_strictSafety_radians);
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+int main(int argc, char* argv[])
+{
+ // Starting the ROS-node
+ ros::init(argc, argv, "FlyingAgentClient");
+
+ // 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 node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber flyingAgentClientConfig_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "FlyingAgentClientConfig", 1, isReadyFlyingAgentClientConfigYamlCallback);
+ ros::Subscriber flyingAgentClientConfig_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("FlyingAgentClientConfig", 1, isReadyFlyingAgentClientConfigYamlCallback);
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters
+ // from the "FlyingAgentClientConfig.yaml" file.
+ // > This is possible because that YAML file is added
+ // to the parameter service when launched via the
+ // "agent.launch" file.
+ fetchFlyingAgentClientConfigYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+
+
+
+
+ // INITIALISE ALL THE CONTROLLER SERVICE CLIENTS
+ // > This cannot be done directly here because the other nodes may
+ // be currently unavailable. Hence, we start a timer for a few
+ // seconds and in the call back all the controller service
+ // clients are created.
+ m_controllers_avialble = false;
+ m_timer_for_createAllControllerServiceClients = nodeHandle.createTimer(ros::Duration(3), timerCallback_for_createAllcontrollerServiceClients, true);
+
+
+
+ // INITIALISE THE MOTION CAPTURE TIME-OUT TIMER
+ // > And stop it immediately
+ m_isAvailable_mocap_data = false;
+ m_timer_mocap_timeout_check = nodeHandle.createTimer(ros::Duration(yaml_mocap_timeout_duration), timerCallback_mocap_timeout_check, true);
+ m_timer_mocap_timeout_check.stop();
+
+
+
+ // INITIALISE THE TAKE-OFF AND LANDING COMPLETE TIMERS
+ // > And stop it immediately
+ m_timer_takeoff_complete = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_takeoff_complete, true);
+ m_timer_takeoff_complete.stop();
+ m_timer_land_complete = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_land_complete, true);
+ m_timer_land_complete.stop();
+
+
+
+ // INITIALISE THE CRAZY RADIO STATUS
+ m_crazyradio_status = CRAZY_RADIO_STATE_DISCONNECTED;
+
+
+
+ // INITIALISE THE FLYING STATE AND PUBLISH
+ m_flying_state = STATE_MOTORS_OFF;
+
+
+
+
+
+ // PUBLISHERS, SUBSCRIBERS, AND SERVICE CLIENTS
+
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+
+ // CREATE A NODE HANDLE TO THE COORDINATOR
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+
+
+ // LOADING OF THIS AGENT'S CONTEXT
+ // Service cleint for loading the allocated flying
+ // zone and other context details
+ //ros::service::waitForService("/CentralManagerService/CentralManager");
+ m_centralManager = nodeHandle_dfall_root.serviceClient<CMQuery>("CentralManagerService/Query", false);
+ // Call the class function that uses this service
+ // client to load the context
+ loadCrazyflieContext();
+ // Subscriber for when the Flying Zone Database changed
+ ros::Subscriber databaseChangedSubscriber = nodeHandle_dfall_root.subscribe("CentralManagerService/DBChanged", 1, crazyflieContextDatabaseChangedCallback);
+
+
+ // EMERGENCY STOP OF THE WHOLE "D-FaLL-System"
+ ros::Subscriber emergencyStopSubscriber = nodeHandle_dfall_root.subscribe("emergencyStop", 1, emergencyStopReceivedCallback);
+
+
+ // LOCALISATION DATA FROM MOTION CAPTURE SYSTEM
+ //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
+ ros::Subscriber viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, viconCallback);
+
+
+
+ // PUBLISHER FOR COMMANDING THE CRAZYFLIE
+ // i.e., motorCmd{1,2,3,4}, roll, pitch, yaw, and onboardControllerType
+ //ros::Publishers to advertise the control output
+ m_commandForSendingToCrazyfliePublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
+
+
+ // SUBSCRIBER FOR THE CHANGE STATE COMMANDS
+ // i.e., {TAKE-OFF,LAND,MOTORS-OFF,CONTROLLER SELECTION}
+ // > for the agent GUI
+ ros::Subscriber commandSubscriber_to_agent = nodeHandle.subscribe("Command", 1, flyingStateRequestCallback);
+ // > for the coord GUI
+ ros::Subscriber commandSubscriber_to_coord = nodeHandle_to_coordinator.subscribe("FlyingAgentClient/Command", 1, flyingStateRequestCallback);
+
+
+ // PUBLISHER FOR THE CRAZYRADIO COMMANDS
+ // i.e., {CONNECT,DISCONNECT}
+ // This topic lets us use the terminal to communicate with
+ // the crazyRadio node even when the GUI is not launched
+ m_crazyRadioCommandPublisher = nodeHandle.advertise<IntWithHeader>("crazyRadioCommand", 1);
+
+
+ // PUBLISHER FOR THE FLYING STATE
+ // Possible states: {MOTORS-OFF,TAKE-OFF,FLYING,LAND}
+ // This topic will publish flying state whenever it changes.
+ m_flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
+
+
+ // PUBLISHER FOR THE CONTROLLER CURRENTLY IN USE
+ // This publishes the "m_instant_controller" variable
+ // to reflect the controller that is actually called
+ // when motion capture data is received.
+ m_controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);
+
+
+ // SUBSCRIBER FOR CRAZY RADIO STATUS CHANGES
+ // crazyradio status. Connected, connecting or disconnected
+ std::string namespace_to_crazy_radio = m_namespace + "/CrazyRadio";
+ ros::NodeHandle nodeHandle_to_crazy_radio(namespace_to_crazy_radio);
+ ros::Subscriber crazyRadioStatusSubscriber = nodeHandle_to_crazy_radio.subscribe("CrazyRadioStatus", 1, crazyRadioStatusCallback);
+
+
+ // SUBSCRIBER FOR BATTERY STATE CHANGES
+ // The battery state change message from the Battery
+ // Monitor node
+ std::string namespace_to_battery_monitor = m_namespace + "/BatteryMonitor";
+ ros::NodeHandle nodeHandle_to_battery_monitor(namespace_to_battery_monitor);
+ ros::Subscriber CFBatterySubscriber = nodeHandle_to_battery_monitor.subscribe("ChangedStateTo", 1, batteryMonitorStateChangedCallback);
+
+
+ // SERVICE SERVER FOR OTHERS TO GET THE CURRENT FLYING STATE
+ // Advertise the service that return the "m_flying_state"
+ // variable when called upon
+ ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("getCurrentFlyingState", getCurrentFlyingStateServiceCallback);
+
+
+ // SERVICE SERVER FOR OTHERS TO GET THE INSTANT CONTROLLER
+ // Advertise the service that return the "m_instant_controller"
+ // variable when called upon
+ ros::ServiceServer getInstantControllerService = nodeHandle.advertiseService("getInstantcontroller", getInstantControllerServiceCallback);
+
+
+
+
+
+ // PUBLISH THE FLYING STATE
+ // Ideally the GUI receives this message
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ m_flyingStatePublisher.publish(flying_state_msg);
+
+
+ // SET THE INITIAL CONTROLLER
+ // This cannot be done before the publishers because
+ // the function also sets the "m_instant_controller"
+ // (as the "m_flying_state" is "STATE_MOTORS_OFF")
+ // and the function that sets the instant controller
+ // publishes a message with the information.
+ setControllerNominallySelected(DEFAULT_CONTROLLER);
+
+
+
+
+
+ // Print out some information to the user.
+ ROS_INFO("[FLYING AGENT CLIENT] 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/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp
similarity index 93%
rename from pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
rename to dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp
index 9195d629a3266051b94d64da16535a1f4ae22faf..9a0545e31edbb227f31deb314f7443a02fe59a6e 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp
@@ -51,14 +51,14 @@
#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 "dfall_pkg/ViconData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
#include <std_msgs/Int32.h>
@@ -76,9 +76,6 @@
// 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.
@@ -122,7 +119,7 @@ float estimator_frequency = 200;
#define LQR_ANGLE_MODE 2
// Namespacing the package
-using namespace d_fall_pps;
+using namespace dfall_pkg;
// ----------------------------------------------------------------------------------
@@ -320,7 +317,7 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&st
// 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 the classroom 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.
@@ -364,7 +361,7 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&st
//
//
//
-// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+// This function WILL NEED TO BE edited for successful completion of the classroom exercise
void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], int Ts_div, Controller::Response &response)
{
@@ -602,7 +599,7 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&st
// 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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
float computeMotorPolyBackward(float thrust)
{
return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
@@ -626,7 +623,7 @@ float computeMotorPolyBackward(float thrust)
// 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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
void setpointCallback(const Setpoint& newSetpoint)
{
setpoint[0] = newSetpoint.x;
@@ -653,53 +650,53 @@ void setpointCallback(const Setpoint& newSetpoint)
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom 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;
- }
- }
+ // // 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
+// This function CAN BE edited for successful completion of the classroom 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)
@@ -749,7 +746,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
}
-// This function CAN BE edited for successful completion of the PPS exercise, and the
+// This function CAN BE edited for successful completion of the classroom 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()
@@ -772,7 +769,7 @@ void processFetchedParameters()
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
{
float val;
@@ -782,7 +779,7 @@ float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
}
return val;
}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
@@ -792,7 +789,7 @@ void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std:
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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
{
int val;
@@ -802,7 +799,7 @@ int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
}
return val;
}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
@@ -812,7 +809,7 @@ void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::stri
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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
{
if(!nodeHandle.getParam(name, val)){
@@ -820,7 +817,7 @@ int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::str
}
return val.size();
}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
{
bool val;
@@ -844,7 +841,7 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
// M M A A III N N
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
int main(int argc, char* argv[]) {
// Starting the ROS-node
@@ -863,16 +860,16 @@ int main(int argc, char* argv[]) {
// > 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"
+ // This line of code adds a parameter named "studentID" to the "FlyingAgentClient"
// > 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");
+ // to the "FlyingAgentClient" node within which this controller service is nested.
+ // Get the handle to the "FlyingAgentClient" node
+ ros::NodeHandle FlyingAgentClient_nodeHandle(m_namespace + "/FlyingAgentClient");
// Get the value of the "studentID" parameter into the instance variable "my_agentID"
- if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
+ if(!FlyingAgentClient_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");
+ ROS_ERROR("[MPC CONTROLLER] Failed to get agentID from FlyingAgentClient");
}
@@ -959,8 +956,8 @@ int main(int argc, char* argv[]) {
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;"
+ // to the name space of this node, i.e., "dfall_pkg" as specified by the line:
+ // "using namespace dfall_pkg;"
// in the "DEFINES" section at the top of this file.
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..854b71acb5f26e98c8cbbbeae6ea92956dc8b577
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
@@ -0,0 +1,349 @@
+// Copyright (C) 2019, 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
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/ParameterService.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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+
+bool requestLoadYamlFilenameCallbackServiceCallback(LoadYamlFromFilename::Request &request, LoadYamlFromFilename::Response &response)
+{
+ // Put the flying state into the response variable
+ requestLoadYamlFilenamePublisher.publish(request.stringWithHeader);
+
+ // Put success into the response
+ response.data = 1;
+
+ // Return
+ return true;
+}
+
+void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_load_with_header)
+{
+ // LOAD THE YAML FILE
+
+ // Get the yaml file name requested
+ std::string yaml_filename_to_load = yaml_filename_to_load_with_header.data;
+ // Get the node handle to this parameter service
+ ros::NodeHandle nodeHandle("~");
+ // Instantiate a local variable for the command string that will be passed to the "system":
+ std::string cmd;
+ // Get the abolute path to "dfall_pkg":
+ std::string dfall_pkg_path = ros::package::getPath("dfall_pkg");
+ // Construct the system command string for (re-)loading the parameters:
+ cmd = "rosparam load " + dfall_pkg_path + "/param" + "/" + yaml_filename_to_load + ".yaml " + m_base_namespace + "/" + yaml_filename_to_load;
+ // 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: " << dfall_pkg_path << " The comand line string sent to the 'system' is: " << cmd );
+ // Send the "load yaml" command to the system
+ system(cmd.c_str());
+
+
+
+ // PUBLISH A MESSAGE THAT THE YAML FILE WAS LOADED
+
+ // Create publisher as a local variable, using the filename
+ // as the name of the message
+ ros::Publisher yamlParametersReadyForFetchPublisher = nodeHandle.advertise<IntWithHeader>(yaml_filename_to_load, 1, true);
+ //ros::spinOnce();
+ // Create a local variable for the message
+ IntWithHeader yaml_ready_msg;
+ // Specify with the data the "type" of this parameter service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ yaml_ready_msg.data = LOAD_YAML_FROM_COORDINATOR;
+ break;
+ }
+ default:
+ {
+ // Throw an error if the type is not recognised
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
+ // Specify to load from the agent by default
+ yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ break;
+ }
+ }
+ // Copy across the boolean field
+ yaml_ready_msg.shouldCheckForAgentID = yaml_filename_to_load_with_header.shouldCheckForAgentID;
+ // Copy across the vector of IDs
+ if (yaml_filename_to_load_with_header.shouldCheckForAgentID)
+ {
+ for ( int i_ID=0 ; i_ID<yaml_filename_to_load_with_header.agentIDs.size() ; i_ID++ )
+ {
+ yaml_ready_msg.agentIDs.push_back(yaml_filename_to_load_with_header.agentIDs[i_ID]);
+ }
+ }
+ // Sleep to make the publisher known to ROS (in seconds)
+ ros::Duration(1.0).sleep();
+ // Send the message
+ yamlParametersReadyForFetchPublisher.publish(yaml_ready_msg);
+
+ // Inform the user that this was published
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Published message that " << yaml_filename_to_load << " yaml parameters are ready." );
+}
+
+
+
+
+
+bool getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 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("[PARAMETER SERVICE] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+
+
+
+
+
+bool constructNamespaces()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Get the namespace of this "ParameterService" node
+ std::string this_node_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() = " << this_node_namespace);
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/agent" + m_Id + '/' + "ParameterService";
+ m_base_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/ParameterService";
+ m_base_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' type parameter was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+int main(int argc, char* argv[])
+{
+ // Starting the ROS-node
+ ros::init(argc, argv, "ParameterService");
+
+ // 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 type and ID of this parameter service
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Construct the namespace into which this parameter service
+ // loads yaml parameters
+ bool isValid_namespaces = constructNamespaces();
+
+ // Stall if the TYPE and ID are not valid
+ if ( !( isValid_type_and_ID && isValid_namespaces ) )
+ {
+ ROS_ERROR("[PARAMETER SERVICE] Service NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+ // Subscribe to the messages that request loading a yaml file
+ ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 1, requestLoadYamlFilenameCallback);
+
+ // Publisher for publishing "internally" to the subscriber above
+ requestLoadYamlFilenamePublisher = nodeHandle.advertise <StringWithHeader>("requestLoadYamlFilename", 1);
+
+ // Advertise the service for requests to load a yaml file
+ ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("requestLoadYamlFilename", requestLoadYamlFilenameCallbackServiceCallback);
+
+ // Inform the user the this node is ready
+ ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
+ // Spin as a single-thread node
+ ros::spin();
+
+ return 0;
+}
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d28bc65675b30c43d80ab94240d15cbb646d92a
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
@@ -0,0 +1,1451 @@
+// Copyright (C) 2019, 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/PickerControllerService.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
+// ----------------------------------------------------------------------------------
+
+
+
+
+// REMINDER OF THE NAME OF USEFUL CLASS VARIABLE
+// // > Mass of the Crazyflie quad-rotor, in [grams]
+// float m_mass_CF_grams;
+// // > Mass of the letters to be lifted, in [grams]
+// float m_mass_E_grams;
+// float m_mass_T_grams;
+// float m_mass_H_grams;
+// // > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
+// float m_mass_total_grams;
+// // Thickness of the object at pick-up and put-down, in [meters]
+// // > This should also account for extra height due to
+// // the surface where the object is
+// float m_thickness_of_object_at_pickup;
+// float m_thickness_of_object_at_putdown;
+// // (x,y) coordinates of the pickup location
+// std::vector<float> m_pickup_coordinates_xy(2);
+// // (x,y) coordinates of the drop off location
+// std::vector<float> m_dropoff_coordinates_xy_for_E(2);
+// std::vector<float> m_dropoff_coordinates_xy_for_T(2);
+// std::vector<float> m_dropoff_coordinates_xy_for_H(2);
+// // Length of the string from the Crazyflie
+// // to the end of the Picker, in [meters]
+// float m_picker_string_length;
+// // > The setpoints for (x,y,z) position and yaw angle, in that order
+// float m_setpoint[4] = {0.0,0.0,0.4,0.0};
+// float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
+// // > Small adjustments to the x-y setpoint
+// float m_xAdjustment = 0.0f;
+// float m_yAdjustment = 0.0f;
+// // Boolean for whether to limit rate of change of the setpoint
+// bool m_shouldSmoothSetpointChanges = true;
+// // Max setpoint change per second
+// float yaml_max_setpoint_change_per_second_horizontal;
+// float yaml_max_setpoint_change_per_second_vertical;
+// float yaml_max_setpoint_change_per_second_yaw_degrees;
+// float m_max_setpoint_change_per_second_yaw_radians;
+// // Frequency at which the controller is running
+// float yaml_control_frequency;
+
+
+// A FEW EXTRA COMMENTS ABOUT THE MOST IMPORTANT VARIABLES
+
+// Variable name: m_setpoint
+// Description:
+// This is a float array of length 4. It specifies a location
+// in space where you want the drone to be. The 4 element are:
+// >> m_setpoint[0] The x-poistion in [meters]
+// >> m_setpoint[1] The y-poistion in [meters]
+// >> m_setpoint[2] The z-poistion in [meters]
+// >> m_setpoint[3] The yaw heading angle in [radians]
+
+
+// Variable name: m_setpoint_for_controller
+// Description:
+// Similar to the variable "m_setpoint" this is also float array
+// of length 4 that specifies an (x,y,z,yaw) location. The
+// difference it that this variable specifies the location where
+// the low-level controller is guiding the drone to be.
+// HINT: to make changes the "m_setpoint" variable, you can edit
+// the function named "perControlCycleOperations" so that the
+// "m_setpoint_for_controller" changes by a maximum amount at
+// each cycle of the contoller
+
+
+
+// THIS FUNCTION IS CALLED AT "m_control_frequency" HERTZ.
+// IT CAN BE USED TO ADJUST THINGS IN "REAL TIME".
+// For example, the equation:
+// >> yaml_max_setpoint_change_per_second_horizontal / m_control_frequency
+// will convert the "change per second" to a "change per cycle".
+
+void perControlCycleOperations()
+{
+
+}
+
+
+
+
+
+
+
+// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
+
+// These functions are called from:
+// >> "requestSetpointChangeCallback"
+// And in that function the following variable are already
+// updated appropriately:
+// >> "m_picker_current_state"
+// >> "m_mass_total_grams"
+// >> "m_shouldSmoothSetpointChanges"
+
+void buttonPressed_goto_start()
+{
+ ROS_INFO("[PICKER CONTROLLER] Goto Start button pressed");
+}
+
+void buttonPressed_attach()
+{
+ ROS_INFO("[PICKER CONTROLLER] Attach button pressed");
+}
+
+void buttonPressed_lift_up()
+{
+ ROS_INFO("[PICKER CONTROLLER] Pick up button pressed");
+}
+
+void buttonPressed_goto_end()
+{
+ ROS_INFO("[PICKER CONTROLLER] Goto End button pressed");
+
+}
+
+void buttonPressed_put_down()
+{
+ ROS_INFO("[PICKER CONTROLLER] Put down button pressed");
+}
+
+void buttonPressed_squat()
+{
+ ROS_INFO("[PICKER CONTROLLER] Squat button pressed");
+}
+
+void buttonPressed_jump()
+{
+ ROS_INFO("[PICKER CONTROLLER] Jump button pressed");
+}
+
+void buttonPressed_standby()
+{
+ ROS_INFO("[PICKER CONTROLLER] Standby button pressed");
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+void smoothSetpointChanges()
+{
+ if (m_shouldSmoothSetpointChanges)
+ {
+ for(int i = 0; i < 4; ++i)
+ {
+ float max_for_this_coordinate;
+ // FILL IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (i)
+ {
+ case 0:
+ max_for_this_coordinate = yaml_max_setpoint_change_per_second_horizontal / yaml_control_frequency;
+ break;
+ case 1:
+ max_for_this_coordinate = yaml_max_setpoint_change_per_second_horizontal / yaml_control_frequency;
+ break;
+ case 2:
+ max_for_this_coordinate = yaml_max_setpoint_change_per_second_vertical / yaml_control_frequency;
+ break;
+ case 3:
+ max_for_this_coordinate = m_max_setpoint_change_per_second_yaw_radians / yaml_control_frequency;
+ break;
+ // Handle the exception
+ default:
+ max_for_this_coordinate = 0.0f;
+ break;
+ }
+
+ // Compute the difference in setpoint
+ float setpoint_difference = m_setpoint[i] - m_setpoint_for_controller[i];
+
+ // Clip the difference to the maximum
+ if (setpoint_difference > max_for_this_coordinate)
+ {
+ setpoint_difference = max_for_this_coordinate;
+ }
+ else if (setpoint_difference < -max_for_this_coordinate)
+ {
+ setpoint_difference = -max_for_this_coordinate;
+ }
+
+ // Update the setpoint of the controller
+ m_setpoint_for_controller[i] += setpoint_difference;
+ }
+
+ }
+ else
+ {
+ m_setpoint_for_controller[0] = m_setpoint[0];
+ m_setpoint_for_controller[1] = m_setpoint[1];
+ m_setpoint_for_controller[2] = m_setpoint[2];
+ m_setpoint_for_controller[3] = m_setpoint[3];
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// 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 "PickerController" 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 classroom exercise
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+
+ // Keep track of time
+ //m_time_ticks++;
+ //m_time_seconds = float(m_time_ticks) / yaml_control_frequency;
+
+
+ // CALL THE FUNCTION FOR PER CYLCE OPERATIONS
+ perControlCycleOperations();
+
+
+ // 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
+ // "m_current_stateInertialEstimate" is updated and ready
+ // to be used for subsequent controller copmutations
+ performEstimatorUpdate_forStateInterial(request);
+
+
+ // SMOOTH ANY CHANGES THAT MAY HAVE OCCURRED IN THE
+ // SETPOINT
+ smoothSetpointChanges();
+
+
+ // 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(m_current_stateInertialEstimate,m_setpoint_for_controller,current_bodyFrameError);
+
+
+ // CARRY OUT THE CONTROLLER COMPUTATIONS
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforRates(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 (yaml_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
+ m_current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ m_current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ m_current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ m_current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ m_current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ m_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 (yaml_estimator_method)
+ {
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ m_current_stateInertialEstimate[i] = m_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)
+ {
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "yaml_estimator_method" was not recognised
+ ROS_INFO_STREAM("[PICKER CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'PickerControllerService': the 'yaml_estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default
+ for(int i = 0; i < 12; ++i)
+ {
+ m_current_stateInertialEstimate[i] = m_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
+ m_previous_xzy_rpy_measurement[0] = m_current_xzy_rpy_measurement[0];
+ m_previous_xzy_rpy_measurement[1] = m_current_xzy_rpy_measurement[1];
+ m_previous_xzy_rpy_measurement[2] = m_current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ m_previous_xzy_rpy_measurement[3] = m_current_xzy_rpy_measurement[3];
+ m_previous_xzy_rpy_measurement[4] = m_current_xzy_rpy_measurement[4];
+ m_previous_xzy_rpy_measurement[5] = m_current_xzy_rpy_measurement[5];
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+{
+ // PUT IN THE CURRENT MEASUREMENT DIRECTLY
+ // > for (x,y,z) position
+ m_stateInterialEstimate_viaFiniteDifference[0] = m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaFiniteDifference[1] = m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaFiniteDifference[2] = m_current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ m_stateInterialEstimate_viaFiniteDifference[6] = m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaFiniteDifference[7] = m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaFiniteDifference[8] = m_current_xzy_rpy_measurement[5];
+
+ // COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
+ // > for (x,y,z) velocities
+ m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ m_stateInterialEstimate_viaFiniteDifference[9] = (m_current_xzy_rpy_measurement[3] - m_previous_xzy_rpy_measurement[3]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[10] = (m_current_xzy_rpy_measurement[4] - m_previous_xzy_rpy_measurement[4]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[11] = (m_current_xzy_rpy_measurement[5] - m_previous_xzy_rpy_measurement[5]) * m_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] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // > Now perform update for:
+ // > x position and velocity:
+ m_stateInterialEstimate_viaPointMassKalmanFilter[0] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[0];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[3] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[0];
+ // > y position and velocity:
+ m_stateInterialEstimate_viaPointMassKalmanFilter[1] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[1];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[4] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[1];
+ // > z position and velocity:
+ m_stateInterialEstimate_viaPointMassKalmanFilter[2] = yaml_PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + yaml_PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + yaml_PMKF_Kinf_for_positions[0]*m_current_xzy_rpy_measurement[2];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[5] = yaml_PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + yaml_PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + yaml_PMKF_Kinf_for_positions[1]*m_current_xzy_rpy_measurement[2];
+
+ // > roll position and velocity:
+ m_stateInterialEstimate_viaPointMassKalmanFilter[6] = yaml_PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + yaml_PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + yaml_PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[3];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[9] = yaml_PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + yaml_PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + yaml_PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[3];
+ // > pitch position and velocity:
+ m_stateInterialEstimate_viaPointMassKalmanFilter[7] = yaml_PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + yaml_PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + yaml_PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[4];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[10] = yaml_PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + yaml_PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + yaml_PMKF_Kinf_for_angles[1]*m_current_xzy_rpy_measurement[4];
+ // > yaw position and velocity:
+ m_stateInterialEstimate_viaPointMassKalmanFilter[8] = yaml_PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + yaml_PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + yaml_PMKF_Kinf_for_angles[0]*m_current_xzy_rpy_measurement[5];
+ m_stateInterialEstimate_viaPointMassKalmanFilter[11] = yaml_PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + yaml_PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + yaml_PMKF_Kinf_for_angles[1]*m_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_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 -= yaml_gainMatrixRollRate[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= yaml_gainMatrixPitchRate[i] * stateErrorBody[i];
+ // BODY FRAME YAW CONTROLLER
+ yawRate_forResponse -= yaml_gainMatrixYawRate[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= yaml_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;
+ // > Compute the feed-forward force
+ float feed_forward_thrust_per_motor = m_weight_total_in_newtons / 4.0;
+ // > Put in the per motor commands
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+
+
+ // 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 (yaml_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:" << yaml_motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 2:" << yaml_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);
+ }
+}
+
+
+
+// ***********************************************************
+// 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
+
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response)
+{
+
+ // 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"
+ m_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
+// ------------------------------------------------------------------------------
+
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured)
+{
+ if (yaml_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];
+
+ // Clip the z-coordination to +/-0.40 meters
+ if (stateInertial[2] > 0.40f)
+ {
+ stateInertial[2] = 0.40f;
+ }
+ else if (stateInertial[2] < -0.40f)
+ {
+ stateInertial[2] = -0.40f;
+ }
+
+ // 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, clip the error to within some bounds
+ if (yawError>(PI/3))
+ {
+ yawError = (PI/3);
+ }
+ else if (yawError<(-PI/3))
+ {
+ yawError = (-PI/3);
+ }
+ // > Finally, put the "yawError" into the "stateError" variable
+ stateInertial[8] = yawError;
+
+
+
+
+ // 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 classroom 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 classroom exercise
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-bit command signal that generates the "thrust" force
+ float cmd = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd < yaml_cmd_sixteenbit_min)
+ {
+ cmd = 0;
+ }
+ else if (cmd > yaml_cmd_sixteenbit_max)
+ {
+ cmd = yaml_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
+// ----------------------------------------------------------------------------------
+
+
+// REQUEST SETPOINT CHANGE CALLBACK
+// This function does NOT need to be edited
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Check that the ".buttonID" property is
+ // actually one of the possible states
+ int button_index = newSetpoint.buttonID;
+ switch(button_index)
+ {
+ case PICKER_STATE_STANDBY:
+ case PICKER_STATE_GOTO_START:
+ case PICKER_STATE_ATTACH:
+ case PICKER_STATE_LIFT_UP:
+ case PICKER_STATE_GOTO_END:
+ case PICKER_STATE_PUT_DOWN:
+ case PICKER_STATE_SQUAT:
+ case PICKER_STATE_JUMP:
+ {
+ // Call the function for actually setting the setpoint
+ setNewSetpoint(
+ newSetpoint.buttonID,
+ newSetpoint.isChecked,
+ newSetpoint.x,
+ newSetpoint.y,
+ newSetpoint.z,
+ newSetpoint.yaw,
+ newSetpoint.mass
+ );
+ break;
+ }
+ default:
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("[PICKER CONTROLLER] A button pressed message was received in the controller but not recognised, message.data = " << button_index );
+ break;
+ }
+ }
+
+ // Call the specific function for each button
+ switch(button_index)
+ {
+ case PICKER_STATE_STANDBY:
+ {
+ buttonPressed_standby();
+ break;
+ }
+ case PICKER_STATE_GOTO_START:
+ {
+ buttonPressed_goto_start();
+ break;
+ }
+ case PICKER_STATE_ATTACH:
+ {
+ buttonPressed_attach();
+ break;
+ }
+ case PICKER_STATE_LIFT_UP:
+ {
+ buttonPressed_lift_up();
+ break;
+ }
+ case PICKER_STATE_GOTO_END:
+ {
+ buttonPressed_goto_end();
+ break;
+ }
+ case PICKER_STATE_PUT_DOWN:
+ {
+ buttonPressed_put_down();
+ break;
+ }
+ case PICKER_STATE_SQUAT:
+ {
+ buttonPressed_squat();
+ break;
+ }
+ case PICKER_STATE_JUMP:
+ {
+ buttonPressed_jump();
+ break;
+ }
+ }
+ }
+}
+
+
+// CHANGE SETPOINT FUNCTION
+// This function does NOT need to be edited
+void setNewSetpoint(int state, bool should_smooth, float x, float y, float z, float yaw, float mass)
+{
+ // Put the state into the class variable
+ m_picker_current_state = state;
+
+ // Put the smoothing flag in the class variable
+ m_shouldSmoothSetpointChanges = should_smooth;
+
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = x;
+ m_setpoint[1] = y;
+ m_setpoint[2] = z;
+ m_setpoint[3] = yaw;
+
+ // Put the mass into the class variable
+ m_mass_total_in_grams = mass;
+ m_weight_total_in_newtons = m_mass_total_in_grams * (9.81/1000.0);
+
+ // Publish the change so that the network is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.buttonID = state;
+ msg.isChecked = should_smooth;
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw;
+ msg.mass = mass;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
+
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+// This function does NOT need to be edited
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response)
+{
+ // Directly put the current setpoint into the response
+ response.setpointWithHeader.buttonID = m_picker_current_state;
+ response.setpointWithHeader.isChecked = m_shouldSmoothSetpointChanges;
+ response.setpointWithHeader.x = m_setpoint[0];
+ response.setpointWithHeader.y = m_setpoint[1];
+ response.setpointWithHeader.z = m_setpoint[2];
+ response.setpointWithHeader.yaw = m_setpoint[3];
+ response.setpointWithHeader.mass = m_mass_total_in_grams;
+ // Return
+ return true;
+}
+
+
+
+
+
+
+
+
+
+// ************************************************************************************************
+// 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)
+{
+ // SetpointWithHeader 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;
+ // m_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
+// ----------------------------------------------------------------------------------
+
+
+// LOADING OF YAML PARAMETERS
+void isReadyPickerControllerYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[PICKER CONTROLLER] Now fetching the PickerController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[PICKER CONTROLLER] Now fetching the PickerController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[PICKER CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchPickerControllerYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // PickerController.yaml
+
+ // Add the "PickerController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "PickerController");
+
+
+
+ // GET THE PARAMETERS:
+
+ // Max setpoint change per second
+ yaml_max_setpoint_change_per_second_horizontal = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_change_per_second_horizontal");
+ yaml_max_setpoint_change_per_second_vertical = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_change_per_second_vertical");
+ yaml_max_setpoint_change_per_second_yaw_degrees = getParameterFloat(nodeHandle_for_paramaters , "max_setpoint_change_per_second_yaw_degrees");
+
+
+
+ // ------------------------------------------------------
+ // PARAMTERS THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
+
+ // > The mass of the crazyflie
+ yaml_mass_cf_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass_cf_in_grams");
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+
+ // > The boolean for whether to execute the convert into body frame function
+ yaml_shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paramaters, "shouldPerformConvertIntoBodyFrame");
+
+ // // > The boolean indicating whether the (x,y,z,yaw) of this agent should be published
+ // // or not
+ // shouldPublishCurrent_xyz_yaw = getParameterBool(nodeHandle_for_paramaters, "shouldPublishCurrent_xyz_yaw");
+
+ // Boolean indiciating whether the "Debug Message" of this agent should be published or not
+ yaml_shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paramaters, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+ yaml_shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
+
+ // A flag for which estimator to use:
+ yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
+
+ // The LQR Controller parameters for "LQR_MODE_RATE"
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
+
+ // 16-BIT COMMAND LIMITS
+ yaml_cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+ yaml_cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_max");
+
+ // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+ // > For the (x,y,z) position
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
+ // > For the (roll,pitch,yaw) angles
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", yaml_PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", yaml_PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , yaml_PMKF_Kinf_for_angles , 2);
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: the fetched PickerController/max_setpoint_change_per_second_yaw_degrees = " << yaml_max_setpoint_change_per_second_yaw_degrees);
+
+
+
+ // PROCESS THE PARAMTERS
+ // Convert from degrees to radians
+ m_max_setpoint_change_per_second_yaw_radians = DEG2RAD * yaml_max_setpoint_change_per_second_yaw_degrees;
+
+ // Set that the estimator frequency is the same as the control frequency
+ m_estimator_frequency = yaml_control_frequency;
+
+ // DEBUGGING: Print out one of the processed values
+ ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: after processing m_max_setpoint_change_per_second_yaw_radians = " << m_max_setpoint_change_per_second_yaw_radians);
+
+
+}
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 classroom exercise
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "PickerControllerService");
+
+ // 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 "PickerControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[PICKER CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[PICKER CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[PICKER CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[PICKER CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[PICKER CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber pickerContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "PickerController", 1, isReadyPickerControllerYamlCallback);
+ ros::Subscriber pickerContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("PickerController", 1, isReadyPickerControllerYamlCallback);
+
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a serice client
+ // we stall the availability of this node until the
+ // paramter service is ready
+
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "PickerController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyPickerControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[PICKER CONTROLLER] The request load yaml file service call failed.");
+ }
+
+
+
+
+
+ // INITIALISE OTHER VARIABLES AS REQUIRED
+ m_mass_total_in_grams = yaml_mass_cf_in_grams;
+ m_weight_total_in_newtons = m_mass_total_in_grams * (9.81/1000.0);
+
+
+
+
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "requestSetpointChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestSetpointChange" topic and calls the class function
+ // "requestSetpointChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the setpoint is changed
+ // by the user.
+ ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("PickerControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Instantiate the class variable "m_setpointChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "SetpointChanged" and is a message with the structure defined
+ // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current setpoint for this controller.
+ m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
+
+ // Instantiate the local variable "getCurrentSetpointService" to be
+ // a "ros::ServiceServer" type variable that advertises the service
+ // called "GetCurrentSetpoint". This service has the input-output
+ // behaviour defined in the "GetSetpointService.srv" file (located
+ // in the "srv" folder). This service, when called, is provided with
+ // an integer (that is essentially ignored), and is expected to respond
+ // with the current setpoint of the controller. When a request is made
+ // of this service the "getCurrentSetpointCallback" function is called.
+ ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);
+
+
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "PickerController". 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("PickerController", calculateControlOutput);
+
+ // 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 agent 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);
+
+
+ // Print out some information to the user.
+ ROS_INFO("[PICKER 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/RemoteControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
similarity index 95%
rename from pps_ws/src/d_fall_pps/src/nodes/RemoteControllerService.cpp
rename to dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
index d7fa4a2d19872f21da4187ffe81fdb3758f30864..de85865dde6359d0ca47fa5c66a4292a42983054 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/RemoteControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
@@ -115,7 +115,7 @@
// 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 the classroom 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.
@@ -159,7 +159,7 @@
//
//
//
-// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+// This function WILL NEED TO BE edited for successful completion of the classroom exercise
@@ -1089,7 +1089,7 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
// > 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
+ // for successful completion of the classroom exercise
convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
}
@@ -1110,7 +1110,7 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
// 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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
float computeMotorPolyBackward(float thrust)
{
// Compute the 16-bit command signal that generates the "thrust" force
@@ -1146,7 +1146,7 @@ float computeMotorPolyBackward(float thrust)
// 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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
void setpointCallback(const Setpoint& newSetpoint)
{
setpoint[0] = newSetpoint.x;
@@ -1211,53 +1211,53 @@ void setpointCallback(const Setpoint& newSetpoint)
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom 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;
- }
- }
+ // // 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
+// This function CAN BE edited for successful completion of the classroom 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)
@@ -1385,7 +1385,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
}
-// This function CAN BE edited for successful completion of the PPS exercise, and the
+// This function CAN BE edited for successful completion of the classroom 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()
@@ -1426,7 +1426,7 @@ void processFetchedParameters()
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
{
float val;
@@ -1436,7 +1436,7 @@ float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
}
return val;
}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
@@ -1446,7 +1446,7 @@ void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std:
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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
{
int val;
@@ -1456,7 +1456,7 @@ int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
}
return val;
}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
@@ -1466,7 +1466,7 @@ void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::stri
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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
{
if(!nodeHandle.getParam(name, val)){
@@ -1474,7 +1474,7 @@ int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::str
}
return val.size();
}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
{
bool val;
@@ -1484,7 +1484,7 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
}
return val;
}
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
{
std::string val;
@@ -1506,7 +1506,7 @@ std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
// M M A A III N N
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
int main(int argc, char* argv[]) {
// Starting the ROS-node
@@ -1526,16 +1526,16 @@ int main(int argc, char* argv[]) {
// > 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"
+ // This line of code adds a parameter named "studentID" to the "FlyingAgentClient"
// > 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");
+ // to the "FlyingAgentClient" node within which this controller service is nested.
+ // Get the handle to the "FlyingAgentClient" node
+ ros::NodeHandle FlyingAgentClient_nodeHandle(m_namespace + "/FlyingAgentClient");
// Get the value of the "studentID" parameter into the instance variable "my_agentID"
- if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
+ if(!FlyingAgentClient_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");
+ ROS_ERROR("[REMOTE CONTROLLER] Failed to get agentID from FlyingAgentClient");
}
@@ -1641,11 +1641,7 @@ int main(int argc, char* argv[]) {
// 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 :-)");
diff --git a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp
similarity index 90%
rename from pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
rename to dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp
index 6607ee9a43ae794b655342bea64395021b1df61d..e9f1c08cfd6265c2585b4c14244354f0069a6178 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp
@@ -146,7 +146,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
// ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
- response.controlOutput.onboardControllerType = RATE_MODE;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
previousLocation = request.ownCrazyflie;
@@ -232,48 +232,48 @@ void estimateState(Controller::Request &request, float (&est)[9])
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom 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_SAFE_CONTROLLER_FROM_OWN_AGENT:
- {
- // Let the user know that this message was received
- 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
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
- break;
- }
-
- // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR:
- {
- // Let the user know that this message was received
- 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
- fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
- break;
- }
-
- default:
- {
- // Let the user know that the command was not relevant
- //ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded.\r> However the parameters do not relate to this controller, hence nothing will be fetched.");
- break;
- }
- }
+ // // 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_SAFE_CONTROLLER_FROM_OWN_AGENT:
+ // {
+ // // Let the user know that this message was received
+ // 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
+ // fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ // break;
+ // }
+
+ // // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ // case FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR:
+ // {
+ // // Let the user know that this message was received
+ // 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
+ // fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ // break;
+ // }
+
+ // default:
+ // {
+ // // Let the user know that the command was not relevant
+ // //ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded.\r> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ // break;
+ // }
+ // }
}
void fetchYamlParameters(ros::NodeHandle& nodeHandle)
@@ -541,7 +541,7 @@ int main(int argc, char* argv[]) {
ROS_INFO("[SAFE CONTROLLER] Service ready :-)");
// std::string package_path;
- // package_path = ros::package::getPath("d_fall_pps") + "/";
+ // package_path = ros::package::getPath("dfall_pkg") + "/";
// ROS_INFO_STREAM(package_path);
// std::string record_file = package_path + "LoggingSafeController.bag";
// bag.open(record_file, rosbag::bagmode::Write);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5a901340b55b623911c3a332197fdd3374b421a8
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -0,0 +1,1068 @@
+// Copyright (C) 2019, 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/StudentControllerService.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 "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)
+//
+// 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 in the header file, they are:
+// - CF_COMMAND_TYPE_MOTORS
+// - CF_COMMAND_TYPE_RATE
+// - CF_COMMAND_TYPE_ANGLE
+//
+// > For completeing the class exercises it is only required to use
+// the CF_COMMAND_TYPE_RATE option.
+//
+// > For the CF_COMMAND_TYPE_RATE optoin:
+// 1) 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.
+// 2) 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" found in the firmware).
+// 3) 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, the 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 classroom exercise
+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 stateErrorInertial[9];
+
+ // Fill in the (x,y,z) position error
+ stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0];
+ stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
+ stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
+
+ // Compute an estimate of the velocity
+ // > As simply the derivative between the current and previous position
+ stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+
+ // Fill in the roll and pitch angle measurements directly
+ stateErrorInertial[6] = request.ownCrazyflie.roll;
+ stateErrorInertial[7] = request.ownCrazyflie.pitch;
+
+ // 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 = request.ownCrazyflie.yaw - m_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
+ stateErrorInertial[8] = yawError;
+
+
+ // 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 classroom exercise
+ float stateErrorBody[9];
+ convertIntoBodyFrame(stateErrorInertial, stateErrorBody, request.ownCrazyflie.yaw);
+
+
+ // SAVE THE STATE ERROR TO BE USED NEXT TIME THIS FUNCTION IS CALLED
+ // > as we have already used previous error we can now update it update it
+ for(int i = 0; i < 9; ++i)
+ {
+ m_previous_stateErrorInertial[i] = stateErrorInertial[i];
+ }
+
+
+
+
+ // **********************
+ // Y Y A W W
+ // Y Y A A W W
+ // Y A A W W
+ // Y AAAAA W W W
+ // Y A A W W
+ //
+ // YAW CONTROLLER
+
+ // Instantiate the local variable for the yaw rate that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+ float yawRate_forResponse = 0;
+
+ // Perform the "-Kx" LQR computation for the yaw rate to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ yawRate_forResponse -= m_gainMatrixYawRate[i] * stateErrorBody[i];
+ }
+
+ // Put the computed yaw rate into the "response" variable
+ response.controlOutput.yaw = yawRate_forResponse;
+
+
+
+
+ // **************************************
+ // BBBB OOO DDDD Y Y ZZZZZ
+ // B B O O D D Y Y Z
+ // BBBB O O D D Y Z
+ // B B O O D D Y Z
+ // BBBB OOO DDDD Y ZZZZZ
+ //
+ // ALITUDE CONTROLLER (i.e., z-controller)
+
+ // Instantiate the local variable for the thrust adjustment that will be
+ // requested from the Crazyflie's on-baord "inner-loop" controller
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the thrust adjustment to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ thrustAdjustment -= m_gainMatrixThrust[i] * stateErrorBody[i];
+ }
+
+ // Put the computed thrust adjustment into the "response" variable,
+ // as well as adding 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 "m_cf_weight_in_newtons" value is the total thrust required
+ // as feed-forward. Assuming the the Crazyflie is symmetric, this
+ // value is divided by four.
+ float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
+ // > NOTE: the function "computeMotorPolyBackward" converts the input argument
+ // from Newtons to the 16-bit command expected by the Crazyflie.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+
+
+
+ // **************************************
+ // BBBB OOO DDDD Y Y X X
+ // B B O O D D Y Y X X
+ // BBBB O O D D Y X
+ // B B O O D D Y X X
+ // BBBB OOO DDDD Y X X
+ //
+ // BODY FRAME X CONTROLLER
+
+ // Instantiate the local variable for the pitch rate that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+ float pitchRate_forResponse = 0;
+
+ // Perform the "-Kx" LQR computation for the pitch rate to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ pitchRate_forResponse -= m_gainMatrixPitchRate[i] * stateErrorBody[i];
+ }
+
+ // Put the computed pitch rate into the "response" variable
+ response.controlOutput.pitch = pitchRate_forResponse;
+
+
+
+
+ // **************************************
+ // BBBB OOO DDDD Y Y Y Y
+ // B B O O D D Y Y Y Y
+ // BBBB O O D D Y Y
+ // B B O O D D Y Y
+ // BBBB OOO DDDD Y Y
+ //
+ // BODY FRAME Y CONTROLLER
+
+ // Instantiate the local variable for the roll rate that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+
+ // Perform the "-Kx" LQR computation for the roll rate to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ rollRate_forResponse -= m_gainMatrixRollRate[i] * stateErrorBody[i];
+ }
+
+ // Put the computed roll rate into the "response" variable
+ response.controlOutput.roll = rollRate_forResponse;
+
+
+
+ // PREPARE AND RETURN THE VARIABLE "response"
+
+ // Choose the controller type use on-board the Crazyflie,
+ // it can be either be Motor, Rate, or Angle based
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+
+
+
+ // ***********************************************************
+ // 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"
+ m_debugPublisher.publish(debugMsg);
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+
+ // 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-coordinates: " << request.ownCrazyflie.x);
+ // ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
+ // ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
+ // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
+ // ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
+ // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
+ // ROS_INFO_STREAM("Delta t: " << 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:" << yaml_motorPoly[0]);
+ // ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
+ // ROS_INFO_STREAM("motorPoly 2:" << yaml_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);
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+ }
+
+
+
+
+// ------------------------------------------------------------------------------
+// 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
+//
+// This function WILL NEED TO BE edited for successful completion of the classroom exercise
+void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
+{
+ // 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];
+}
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// 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 NEED TO BE edited for successful completion of
+// the exercise
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-bit command that would produce the requested
+ // "thrust" based on the quadratic mapping that is described
+ // by the coefficients in the "yaml_motorPoly" variable.
+ float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
+
+
+
+ // > Could this formula compute a result "cmd_16bit" that is
+ // greater than ((2^16)-1)?
+ // > What might happen when such a number is case as a 16-bit
+ // integer? In other words, what is uint16(cmd_sixteen_bit)?
+
+
+
+ // Return the result
+ return cmd_16bit;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+// REQUEST SETPOINT CHANGE CALLBACK
+// This function does NOT need to be edited
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Check if the request if for the default setpoint
+ if (newSetpoint.buttonID == REQUEST_DEFAULT_SETPOINT_BUTTON_ID)
+ {
+ setNewSetpoint(
+ yaml_default_setpoint[0],
+ yaml_default_setpoint[1],
+ yaml_default_setpoint[2],
+ yaml_default_setpoint[3]
+ );
+ }
+ else
+ {
+ // Call the function for actually setting the setpoint
+ setNewSetpoint(
+ newSetpoint.x,
+ newSetpoint.y,
+ newSetpoint.z,
+ newSetpoint.yaw
+ );
+ }
+ }
+}
+
+
+// CHANGE SETPOINT FUNCTION
+// This function does NOT need to be edited
+void setNewSetpoint(float x, float y, float z, float yaw)
+{
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = x;
+ m_setpoint[1] = y;
+ m_setpoint[2] = z;
+ m_setpoint[3] = yaw;
+
+ // Publish the change so that the network is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
+
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+// This function does NOT need to be edited
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response)
+{
+ // Directly put the current setpoint into the response
+ response.setpointWithHeader.x = m_setpoint[0];
+ response.setpointWithHeader.y = m_setpoint[1];
+ response.setpointWithHeader.z = m_setpoint[2];
+ response.setpointWithHeader.yaw = m_setpoint[3];
+ // Return
+ return true;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// This function CAN be edited to respond when the buttons
+// in the GUI are pressed
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , commandReceived.shouldCheckForAgentID , commandReceived.agentIDs );
+
+ if (isRevelant)
+ {
+ // Extract the data from the message
+ int custom_button_index = commandReceived.button_index;
+ float float_data = commandReceived.float_data;
+ //std::string string_data = commandReceived.string_data;
+
+ // 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_STREAM("[STUDENT CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
+ // 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_STREAM("[STUDENT CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
+ // 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 message.float_data = " << float_data );
+ // Code here to respond to custom button 3
+
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.float_data = " << float_data );
+ break;
+ }
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// TIMER CALLBACK FOR SENDING THE LOAD YAML REQUEST
+// This function does NOT need to be edited
+void timerCallback_initial_load_yaml(const ros::TimerEvent&)
+{
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "StudentController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyStudentControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[STUDENT CONTROLLER] The request load yaml file service call failed.");
+ }
+}
+
+
+// LOADING OF YAML PARAMETERS
+// This function does NOT need to be edited
+void isReadyStudentControllerYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[STUDENT CONTROLLER] Now fetching the StudentController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[STUDENT CONTROLLER] Now fetching the StudentController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[STUDENT CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchStudentControllerYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+// This function CAN BE edited for successful completion of the
+// exercise, and the use of parameters fetched from the YAML file
+// is highly recommended to make tuning of your controller easier
+// and quicker.
+void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // StudentController.yaml
+
+ // Add the "StudentController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "StudentController");
+
+
+
+ // GET THE PARAMETERS:
+
+ // > The mass of the crazyflie
+ yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
+
+ // Display one of the YAML parameters to debug if it is working correctly
+ //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << yaml_cf_mass_in_grams );
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+
+ // The default setpoint, the ordering is (x,y,z,yaw),
+ // with unit [meters,meters,meters,radians]
+ getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+
+
+
+ // > DEBUGGING: Print out one of the parameters that was loaded to
+ // debug if the fetching of parameters worked correctly
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << yaml_cf_mass_in_grams);
+
+
+
+ // PROCESS THE PARAMTERS
+
+ // > Compute the feed-forward force that we need to counteract
+ // gravity (i.e., mg) in units of [Newtons]
+ m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
+
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ 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);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[STUDENT CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber safeContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "StudentController", 1, isReadyStudentControllerYamlCallback);
+ ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("StudentController", 1, isReadyStudentControllerYamlCallback);
+
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+ // > the mass of the crazyflie, in [grams]
+ yaml_cf_mass_in_grams = 25.0;
+ // > the coefficients of the 16-bit command to thrust conversion
+ yaml_motorPoly[0] = 5.484560e-4;
+ yaml_motorPoly[1] = 1.032633e-6;
+ yaml_motorPoly[2] = 2.130295e-11;
+ // > the frequency at which the controller is running
+ yaml_control_frequency = 200.0;
+ // > the default setpoint, the ordering is (x,y,z,yaw),
+ // with units [meters,meters,meters,radians]
+ yaml_default_setpoint[0] = 0.0;
+ yaml_default_setpoint[1] = 0.0;
+ yaml_default_setpoint[2] = 0.4;
+ yaml_default_setpoint[3] = 0.0;
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a serice client
+ // we stall the availability of this node until the
+ // paramter service is ready
+ // > NOTE FURTHER that calling on the service directly from here
+ // often means the yaml file is not actually loaded. So we
+ // instead use a timer to delay the loading
+
+ // Create a single-shot timer
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_initial_load_yaml, true);
+ timer_initial_load_yaml.start();
+
+
+
+
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "requestSetpointChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestSetpointChange" topic and calls the class function
+ // "requestSetpointChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the setpoint is changed
+ // by the user.
+ ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("StudentControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Instantiate the class variable "m_setpointChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "SetpointChanged" and is a message with the structure defined
+ // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current setpoint for this controller.
+ m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
+
+ // Instantiate the local variable "getCurrentSetpointService" to be
+ // a "ros::ServiceServer" type variable that advertises the service
+ // called "GetCurrentSetpoint". This service has the input-output
+ // behaviour defined in the "GetSetpointService.srv" file (located
+ // in the "srv" folder). This service, when called, is provided with
+ // an integer (that is essentially ignored), and is expected to respond
+ // with the current setpoint of the controller. When a request is made
+ // of this service the "getCurrentSetpointCallback" function is called.
+ ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);
+
+
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // 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("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("CustomButtonPressed", 1, customCommandReceivedCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ //std::string namespace_to_coordinator;
+ //constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ //ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("StudentControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
+
+
+
+ // Print out some information to the user.
+ ROS_INFO("[STUDENT 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/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..71ecce97699762b0beec89de03b9b339cc8c9203
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
@@ -0,0 +1,994 @@
+// Copyright (C) 2019, 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:
+// A Template Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/TemplateControllerService.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 "TemplateController"
+// 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
+// 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 motion capture 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 in the header file, they are:
+// - CF_COMMAND_TYPE_MOTORS
+// - CF_COMMAND_TYPE_RATE
+// - CF_COMMAND_TYPE_ANGLE
+//
+// > For most common option to use is CF_COMMAND_TYPE_RATE option.
+//
+// > For the CF_COMMAND_TYPE_RATE optoin:
+// 1) 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.
+// 2) 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" found in the firmware).
+// 3) 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, the 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 out
+// of the screen,
+// > This being a "top view" means that 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)
+// \____/ \____/
+//
+//
+//
+//
+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 stateErrorInertial[9];
+
+ // Fill in the (x,y,z) position error
+ stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0];
+ stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
+ stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
+
+ // Compute an estimate of the velocity
+ // > As simply the derivative between the current and previous position
+ stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+
+ // Fill in the roll and pitch angle measurements directly
+ stateErrorInertial[6] = request.ownCrazyflie.roll;
+ stateErrorInertial[7] = request.ownCrazyflie.pitch;
+
+ // 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 = request.ownCrazyflie.yaw - m_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
+ stateErrorInertial[8] = yawError;
+
+
+ // 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 classroom exercise
+ float stateErrorBody[9];
+ convertIntoBodyFrame(stateErrorInertial, stateErrorBody, request.ownCrazyflie.yaw);
+
+
+ // SAVE THE STATE ERROR TO BE USED NEXT TIME THIS FUNCTION IS CALLED
+ // > as we have already used previous error we can now update it update it
+ for(int i = 0; i < 9; ++i)
+ {
+ m_previous_stateErrorInertial[i] = stateErrorInertial[i];
+ }
+
+
+
+ // PERFORM THE "u=-Kx" CONTROLLER COMPUTATIONS
+
+ // Instantiate local variables for the responses
+ float thrustAdjustment = 0;
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+
+ // Perform the "-Kx" LQR computation for the yaw rate to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ // For the z-controller
+ thrustAdjustment -= yaml_gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ // For the x-controller
+ pitchRate_forResponse -= yaml_gainMatrixPitchRate[i] * stateErrorBody[i];
+ // For the y-controller
+ rollRate_forResponse -= yaml_gainMatrixRollRate[i] * stateErrorBody[i];
+ // For the yaw-controller
+ yawRate_forResponse -= yaml_gainMatrixYawRate[i] * stateErrorBody[i];
+ }
+
+ // Put the computed body rate commands into the "response" variable
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+
+ // Put the thrust commands into the "response" variable.
+ // The thrust adjustment computed by the controller need to be added to the
+ // the feed-forward thrust that "counter-acts" 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 "m_cf_weight_in_newtons" value is the total thrust required
+ // as feed-forward. Assuming the the Crazyflie is symmetric, this
+ // value is divided by four.
+ float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
+ float thrust_request_per_motor = thrustAdjustment / 4.0 + feed_forward_thrust_per_motor;
+ // > NOTE: the function "computeMotorPolyBackward" converts the input argument
+ // from Newtons to the 16-bit command expected by the Crazyflie.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrust_request_per_motor);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrust_request_per_motor);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_request_per_motor);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_request_per_motor);
+
+
+ // PREPARE AND RETURN THE VARIABLE "response"
+
+ // Choose the controller type use on-board the Crazyflie,
+ // it can be either be Motor, Rate, or Angle based
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+
+
+ if (yaml_shouldPublishDebugMessage)
+ {
+ // ***********************************************************
+ // 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"
+ m_debugPublisher.publish(debugMsg);
+ }
+
+
+ if (yaml_shouldDisplayDebugInfo)
+ {
+ // ***********************************************************
+ // 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
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+
+ // 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-coordinates: " << request.ownCrazyflie.x);
+ ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
+ ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
+ ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
+ ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
+ ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
+ ROS_INFO_STREAM("Delta t: " << 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 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);
+
+ // An example of "printing out" the "thrust-to-command" conversion parameters.
+ // ROS_INFO_STREAM("motorPoly 0:" << yaml_motorPoly[0]);
+ // ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
+ // ROS_INFO_STREAM("motorPoly 2:" << yaml_motorPoly[2]);
+ }
+
+ // Return "true" to indicate that the control computation was
+ // performed successfully
+ return true;
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// 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[9], float (&stateBody)[9], float yaw_measured)
+{
+ 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];
+}
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-but command that would produce the requested
+ // "thrust" based on the quadratic mapping that is described
+ // by the coefficients in the "yaml_motorPoly" variable.
+ float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd_16bit < yaml_command_sixteenbit_min)
+ {
+ cmd_16bit = 0;
+ }
+ else if (cmd_16bit > yaml_command_sixteenbit_max)
+ {
+ cmd_16bit = yaml_command_sixteenbit_max;
+ }
+ // Return the result
+ return cmd_16bit;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Check if the request if for the default setpoint
+ if (newSetpoint.buttonID == REQUEST_DEFAULT_SETPOINT_BUTTON_ID)
+ {
+ setNewSetpoint(
+ yaml_default_setpoint[0],
+ yaml_default_setpoint[1],
+ yaml_default_setpoint[2],
+ yaml_default_setpoint[3]
+ );
+ }
+ else
+ {
+ // Call the function for actually setting the setpoint
+ setNewSetpoint(
+ newSetpoint.x,
+ newSetpoint.y,
+ newSetpoint.z,
+ newSetpoint.yaw
+ );
+ }
+ }
+}
+
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw)
+{
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = x;
+ m_setpoint[1] = y;
+ m_setpoint[2] = z;
+ m_setpoint[3] = yaw;
+
+ // Publish the change so that the network is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
+
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response)
+{
+ // Directly put the current setpoint into the response
+ response.setpointWithHeader.x = m_setpoint[0];
+ response.setpointWithHeader.y = m_setpoint[1];
+ response.setpointWithHeader.z = m_setpoint[2];
+ response.setpointWithHeader.yaw = m_setpoint[3];
+ // Return
+ return true;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 CustomButtonWithHeader& commandReceived)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , commandReceived.shouldCheckForAgentID , commandReceived.agentIDs );
+
+ if (isRevelant)
+ {
+ // Extract the data from the message
+ int custom_button_index = commandReceived.button_index;
+ float float_data = commandReceived.float_data;
+ //std::string string_data = commandReceived.string_data;
+
+ // 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_STREAM("[TEMPLATE CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
+ // 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_STREAM("[TEMPLATE CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
+ // 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("[TEMPLATE CONTROLLER] Button 3 received in controller, with message.float_data = " << float_data );
+ // Code here to respond to custom button 3
+
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.float_data = " << float_data );
+ break;
+ }
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// CALLBACK NOTIFYING THAT THE YAML PARAMETERS ARE READY TO BE LOADED
+void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[TEMPLATE CONTROLLER] Now fetching the TemplateController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[TEMPLATE CONTROLLER] Now fetching the TemplateController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[TEMPLATE CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchTemplateControllerYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+// LOADING OF THE YAML PARAMTERS
+void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // TemplateController.yaml
+
+ // Add the "TemplateController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "TemplateController");
+
+
+
+ // GET THE PARAMETERS:
+
+ // > The mass of the crazyflie
+ yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
+
+ // > The frequency at which the "computeControlOutput" is being called,
+ // as determined by the frequency at which the motion capture system
+ // provides position and attitude data
+ yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor
+ // command to thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+
+ // > The min and max for saturating 16 bit thrust commands
+ yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+ yaml_command_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_max");
+
+ // The default setpoint, the ordering is (x,y,z,yaw),
+ // with unit [meters,meters,meters,radians]
+ getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+
+ // Boolean indiciating whether the "Debug Message" of this agent
+ // should be published or not
+ yaml_shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paramaters, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should
+ // be displayed or not
+ yaml_shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
+
+ // The LQR Controller parameters
+ // The LQR Controller parameters for "LQR_MODE_RATE"
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
+
+ // > DEBUGGING: Print out one of the parameters that was loaded to
+ // debug if the fetching of parameters worked correctly
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: the fetched TemplateController/mass = " << yaml_cf_mass_in_grams);
+
+
+
+ // PROCESS THE PARAMTERS
+
+ // > Compute the feed-forward force that we need to counteract
+ // gravity (i.e., mg) in units of [Newtons]
+ m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
+
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "TemplateControllerService");
+
+ // 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 "TemplateControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[TEMPLATE CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[TEMPLATE CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber safeContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "TemplateController", 1, isReadyTemplateControllerYamlCallback);
+ ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("TemplateController", 1, isReadyTemplateControllerYamlCallback);
+
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a serice client
+ // we stall the availability of this node until the
+ // paramter service is ready
+
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "TemplateController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyTemplateControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[TEMPLATE CONTROLLER] The request load yaml file service call failed.");
+ }
+
+
+
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "requestSetpointChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestSetpointChange" topic and calls the class function
+ // "requestSetpointChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the setpoint is changed
+ // by the user.
+ ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("TemplateControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Instantiate the class variable "m_setpointChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "SetpointChanged" and is a message with the structure defined
+ // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current setpoint for this controller.
+ m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
+
+ // Instantiate the local variable "getCurrentSetpointService" to be
+ // a "ros::ServiceServer" type variable that advertises the service
+ // called "GetCurrentSetpoint". This service has the input-output
+ // behaviour defined in the "GetSetpointService.srv" file (located
+ // in the "srv" folder). This service, when called, is provided with
+ // an integer (that is essentially ignored), and is expected to respond
+ // with the current setpoint of the controller. When a request is made
+ // of this service the "getCurrentSetpointCallback" function is called.
+ ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);
+
+
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "TemplateController". 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("TemplateController", 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("CustomButtonPressed", 1, customCommandReceivedCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ //std::string namespace_to_coordinator;
+ //constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ //ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("TemplateControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
+
+
+
+ // Print out some information to the user.
+ ROS_INFO("[TEMPLATE 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/TuningControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
similarity index 74%
rename from pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
rename to dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
index 09bf7db145ee1701a7805768084d7f8f80c29db3..072c0be37200f23564f7b67323643c164cabab7b 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//
// This file is part of D-FaLL-System.
//
@@ -25,7 +25,7 @@
//
//
// DESCRIPTION:
-// Place for students to implement their controller
+// Allows for simple tuning of a P(I)D controller
//
// ----------------------------------------------------------------------------------
@@ -115,7 +115,7 @@
// 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 the classroom 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.
@@ -159,7 +159,7 @@
//
//
//
-// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+// This function WILL NEED TO BE edited for successful completion of the classroom exercise
@@ -914,16 +914,19 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
- lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
+ //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;
+ //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;
}
+ //lqr_angleRateNested_prev_rollAngle -= ( -1 * m_gain_P * stateErrorBody[1] - m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[4] );
+ lqr_angleRateNested_prev_pitchAngle -= ( m_gain_P * stateErrorBody[0] + m_gain_P * m_gain_P_to_D_ratio * stateErrorBody[3] );
+
// BODY FRAME Z CONTROLLER
//lqr_angleRateNested_prev_yawAngle = setpoint[3];
- lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
+ //lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
}
@@ -951,14 +954,29 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
for(int i = 0; i < 4; ++i)
{
// BODY FRAME Y CONTROLLER
- rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
+ //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;
+ //yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i] * multiplier_heading;
+ }
+
+
+
+ float thrustAdjustment_200Hz = 0.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];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment_200Hz -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ // BODY FRAME YAW CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
}
+
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
// Put the computed rates and thrust into the "response" variable
@@ -970,7 +988,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// > 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;
+ float thrustAdjustment = thrustAdjustment_200Hz / 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);
@@ -1046,7 +1064,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
// debugMsg.value_10 = your_variable_name;
// Publish the "debugMsg"
- debugPublisher.publish(debugMsg);
+ m_debugPublisher.publish(debugMsg);
}
@@ -1179,7 +1197,7 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
// > 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
+ // for successful completion of the classroom exercise
convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
}
@@ -1200,7 +1218,7 @@ void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setp
// 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
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
float computeMotorPolyBackward(float thrust)
{
// Compute the 16-bit command signal that generates the "thrust" force
@@ -1236,46 +1254,82 @@ float computeMotorPolyBackward(float thrust)
// 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 classroom exercise
+// void setpointCallback(const Setpoint& newSetpoint)
+// {
+// setpoint[0] = newSetpoint.x;
+// setpoint[1] = newSetpoint.y;
+// setpoint[2] = newSetpoint.z;
+// setpoint[3] = newSetpoint.yaw;
+// }
+// REQUEST SETPOINT CHANGE CALLBACK
+// This function does NOT need to be edited
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Call the function for actually setting the setpoint
+ setNewSetpoint(
+ newSetpoint.x,
+ newSetpoint.y,
+ newSetpoint.z,
+ newSetpoint.yaw
+ );
+ }
+}
+// CHANGE SETPOINT FUNCTION
+// This function does NOT need to be edited
+void setNewSetpoint(float x, float y, float z, float yaw)
+{
+ // Put the new setpoint into the class variable
+ setpoint[0] = x;
+ setpoint[1] = y;
+ setpoint[2] = z;
+ setpoint[3] = yaw;
+
+ // Publish the change so that the network is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // // Instantiate a local variable of type "SetpointWithHeader"
+ // SetpointWithHeader msg;
+ // // Fill in the setpoint
+ // msg.x = x;
+ // msg.y = y;
+ // msg.z = z;
+ // msg.yaw = yaw;
+ // // Publish the message
+ // m_setpointChangedPublisher.publish(msg);
+}
+// REQUEST SETPOINT CHANGE CALLBACK
+// This function does NOT need to be edited
+void requestGainChangeCallback(const FloatWithHeader& newGain)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newGain.shouldCheckForAgentID , newGain.agentIDs );
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ m_gain_P = newGain.data;
+ ROS_INFO_STREAM("[TUNING CONTROLLER] proportional gain changed to " << m_gain_P );
+ }
+}
@@ -1301,98 +1355,99 @@ void setpointCallback(const Setpoint& newSetpoint)
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+// LOADING OF YAML PARAMETERS
+// This function does NOT need to be edited
+void isReadyTuningControllerYamlCallback(const IntWithHeader & msg)
{
- // Extract from the "msg" for which controller the and from where to fetch the YAML
- // parameters
- int controller_to_fetch_yaml = msg.data;
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
- // Switch between fetching for the different controllers and from different locations
- switch(controller_to_fetch_yaml)
+ // Continue if the message is relevant
+ if (isRevelant)
{
-
- // > 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:
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
{
- // 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;
- }
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[TUNING CONTROLLER] Now fetching the TuningController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[TUNING CONTROLLER] Now fetching the TuningController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_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;
+ default:
+ {
+ ROS_ERROR("[TUNING CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
}
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchTuningControllerYamlParameters(nodeHandle_to_use);
}
}
-// This function CAN BE edited for successful completion of the PPS exercise, and the
+// This function CAN BE edited for successful completion of the classroom 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)
+void fetchTuningControllerYamlParameters(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");
+ // Add the "TuningController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "test_horizontal_setpoint1", test_horizontal_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_paramaters, "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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "test_vertical_setpoint1", test_vertical_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_paramaters, "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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "test_heading_setpoint1", test_heading_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_paramaters, "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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "test_all_setpoint1", test_all_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_paramaters, "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");
+ test_circle_radius = getParameterFloat(nodeHandle_for_paramaters, "test_circle_radius");
+ test_circle_seconds_per_rev = getParameterFloat(nodeHandle_for_paramaters, "test_circle_seconds_per_rev");
+ test_circle_height = getParameterFloat(nodeHandle_for_paramaters, "test_circle_height");
+ test_circle_time_to_reach_start = getParameterFloat(nodeHandle_for_paramaters, "test_circle_time_to_reach_start");
+ test_circle_pirouette_per_rev = getParameterFloat(nodeHandle_for_paramaters, "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");
+ multiplier_horizontal_min = getParameterFloat(nodeHandle_for_paramaters, "multiplier_horizontal_min");
+ multiplier_horizontal_max = getParameterFloat(nodeHandle_for_paramaters, "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");
+ multiplier_vertical_min = getParameterFloat(nodeHandle_for_paramaters, "multiplier_vertical_min");
+ multiplier_vertical_max = getParameterFloat(nodeHandle_for_paramaters, "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");
+ multiplier_heading_min = getParameterFloat(nodeHandle_for_paramaters, "multiplier_heading_min");
+ multiplier_heading_max = getParameterFloat(nodeHandle_for_paramaters, "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);
@@ -1402,76 +1457,76 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > The mass of the crazyflie
- cf_mass = getParameterFloat(nodeHandle_for_TuningController , "mass");
+ cf_mass = getParameterFloat(nodeHandle_for_paramaters , "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");
+ vicon_frequency = getParameterFloat(nodeHandle_for_paramaters, "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");
+ control_frequency = getParameterFloat(nodeHandle_for_paramaters, "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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
- shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_TuningController, "shouldPerformConvertIntoBodyFrame");
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paramaters, "shouldPerformConvertIntoBodyFrame");
// Boolean indiciating whether the "Debug Message" of this agent should be published or not
- shouldPublishDebugMessage = getParameterBool(nodeHandle_for_TuningController, "shouldPublishDebugMessage");
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paramaters, "shouldPublishDebugMessage");
// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
- shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_TuningController, "shouldDisplayDebugInfo");
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
// THE CONTROLLER GAINS:
// A flag for which controller to use:
- controller_mode = getParameterInt( nodeHandle_for_TuningController , "controller_mode" );
+ controller_mode = getParameterInt( nodeHandle_for_paramaters , "controller_mode" );
// A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_TuningController , "estimator_method" );
+ estimator_method = getParameterInt( nodeHandle_for_paramaters , "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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "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_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "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");
+ cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+ cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "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);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
@@ -1480,15 +1535,8 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// Call the function that computes details an values that are needed from these
// parameters loaded above
- processFetchedParameters();
-}
+ //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);
@@ -1503,82 +1551,6 @@ void processFetchedParameters()
-// ----------------------------------------------------------------------------------
-// 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;
-}
@@ -1592,123 +1564,206 @@ std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
// M M A A III N N
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// This function DOES NOT NEED TO BE edited for successful completion of the classroom 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");
- }
+ // 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 "TuningControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
- // *********************************************************************************
- // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+ // AGENT ID AND COORDINATOR ID
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
- // 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";
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[TUNING CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[TUNING CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
- // 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);
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[TUNING CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[TUNING CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber safeContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "TuningController", 1, isReadyTuningControllerYamlCallback);
+ ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("TuningController", 1, isReadyTuningControllerYamlCallback);
+
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+ // // > the mass of the crazyflie, in [grams]
+ // yaml_cf_mass_in_grams = 25.0;
+ // // > the coefficients of the 16-bit command to thrust conversion
+ // yaml_motorPoly[0] = 5.484560e-4;
+ // yaml_motorPoly[1] = 1.032633e-6;
+ // yaml_motorPoly[2] = 2.130295e-11;
+ // // > the frequency at which the controller is running
+ // yaml_control_frequency = 200.0;
+ // // > the default setpoint, the ordering is (x,y,z,yaw),
+ // // with units [meters,meters,meters,radians]
+ // yaml_default_setpoint[0] = 0.0;
+ // yaml_default_setpoint[1] = 0.0;
+ // yaml_default_setpoint[2] = 0.4;
+ // yaml_default_setpoint[3] = 0.0;
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a serice client
+ // we stall the availability of this node until the
+ // paramter service is ready
+
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "TuningController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyTuningControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[TUNING CONTROLLER] The request load yaml file service call failed.");
+ }
- // 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);
+ // // 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);
- // 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);
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
- // 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 local variable "requestSetpointChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestSetpointChange" topic and calls the class function
+ // "requestSetpointChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the setpoint is changed
+ // by the user.
+ ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("TuningControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+ // Instantiate the local variable "requestGainChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestGainChange" topic and calls the class function
+ // "requestGainChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the gain is changed
+ // by the user.
+ ros::Subscriber requestGainChangeSubscriber = nodeHandle.subscribe("RequestGainChange", 1, requestGainChangeCallback);
- // 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
+ // variable that advertises the service called "TuningController". 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
@@ -1717,11 +1772,7 @@ int main(int argc, char* argv[]) {
// 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 :-)");
diff --git a/pps_ws/src/d_fall_pps/src/nodes/ViconDataPublisher.cpp b/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
similarity index 61%
rename from pps_ws/src/d_fall_pps/src/nodes/ViconDataPublisher.cpp
rename to dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
index 2c9170a6a0225b6d8b12a3422e36ae0339c2d70c..1361db48e83642531859073e5810d176f8fe5262 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/ViconDataPublisher.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
@@ -1,4 +1,4 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Dusan Zikovic, Cyrill Burgener, Marco Mueller, Philipp Friedli
+// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero, Dusan Zikovic, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
// This file is part of D-FaLL-System.
//
@@ -33,155 +33,138 @@
#include <string.h>
#include "DataStreamClient.h"
#include "ros/ros.h"
-#include "d_fall_pps/ViconData.h"
-#include "d_fall_pps/UnlabeledMarker.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/UnlabeledMarker.h"
// #define TESTING_FAKE_DATA
// notice that unit here are in milimeters
using namespace ViconDataStreamSDK::CPP;
-using namespace d_fall_pps;
+using namespace dfall_pkg;
int main(int argc, char* argv[]) {
- ros::init(argc, argv, "ViconDataPublisher");
-
- ros::NodeHandle nodeHandle("~");
- ros::Time::init();
- ros::Publisher viconDataPublisher =
- nodeHandle.advertise<ViconData>("ViconData", 1);
- #ifdef TESTING_FAKE_DATA
- // Test faking data part
- float f = 0;
- int i = 0;
-
- ROS_INFO("TESTING_FAKE_DATA.................................");
- while(ros::ok())
- {
- if(i % 1000 == 0)
- {
- ROS_INFO("iteration #%d",i);
- }
-
- // Testing piece of code
- ViconData viconData;
- UnlabeledMarker marker;
+ // Starting the ROS-node
+ ros::init(argc, argv, "ViconDataPublisher");
- marker.index = 0;
- marker.x = f;
- marker.y = 0;
- marker.z = 0;
- viconData.markers.push_back(marker);
+ // 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("~");
- marker.index = 1;
- marker.x = 0;
- marker.y = f;
- marker.z = 0;
- viconData.markers.push_back(marker);
- if(i > 50 && i < 100)
- {
- marker.index = 2;
- marker.x = f;
- marker.y = f;
- marker.z = 0;
- viconData.markers.push_back(marker);
- }
+ // Initialise ROS time
+ ros::Time::init();
- ros::Duration(0.1).sleep();
- f += 10/1000.0f;
- i++;
- // TODO: Fake CF data
- CrazyflieData crazyfly;
- crazyfly.occluded = false;
- crazyfly.crazyflieName = "CF1";
- crazyfly.x = 0;
- crazyfly.y = 0;
- crazyfly.z = 0;
- crazyfly.yaw = 3.14159 * f;
- viconData.crazyflies.push_back(crazyfly);
+ // PUBLISHER FOR THE MOTION CAPTURE DATA
+ // > Created as a local variable becuase the "looping" of
+ // ros is handled directly in the "while (ros::ok())"
+ // loop below, i.e., this variable will not
+ // go out-of-scope
+ ros::Publisher viconDataPublisher =
+ nodeHandle.advertise<ViconData>("ViconData", 1);
- crazyfly.crazyflieName = "CF2";
- crazyfly.x = 1;
- crazyfly.y = 1;
- crazyfly.z = 0;
- crazyfly.yaw = -3.14159 * f;
- viconData.crazyflies.push_back(crazyfly);
- crazyfly.crazyflieName = "CF3";
- crazyfly.x = 1;
- crazyfly.y = -1;
- crazyfly.z = 0;
- crazyfly.yaw = -3.14159 * f;
+#ifdef TESTING_FAKE_DATA
+ testFakeData(viconDataPublisher);
+#else
- if(i > 50 && i < 200)
- {
- crazyfly.occluded = true;
- }
- viconData.crazyflies.push_back(crazyfly);
- viconDataPublisher.publish(viconData); // testing data
- }
- #else
+ // CLIENT FOR GETTING DATA FROM THE VICON DATA STREAM SDK
+ // > The "Client" variable type is defined in the header
+ // "DataStreamClient.h"
+ Client vicon_client;
- Client client;
+ // HOSTNAME (IP ADDRESS) OF THE "VICON COMPUTER"
+ // > This is specified in the "ViconConfig.yaml" file
+ // > That yaml file is added to this node during launch
+ // > The "Vicon Computer" runs the "Tacker" software that
+ // is the heart of the Vicon Motion Capture system
std::string hostName;
if(!nodeHandle.getParam("hostName", hostName)) {
ROS_ERROR("Failed to get hostName");
return 1;
}
- ROS_INFO_STREAM("Connecting to " << hostName << " ...");
- while (!client.IsConnected().Connected) {
- bool ok = (client.Connect(hostName).Result == Result::Success);
- if (!ok) {
- ROS_ERROR("Error - connection failed...");
+
+ // CONNECT TO THE HOST (i.e., TO THE "VICON COMPUTER")
+
+ // Inform the user this is about to be attempted
+ ROS_INFO_STREAM("[VICON DATA PUBLISHER] Connecting to Vicon host with name: " << hostName );
+
+ // Attempt to connect in a while loop
+ while (!vicon_client.IsConnected().Connected)
+ {
+ // Get the connection status
+ bool ok = (vicon_client.Connect(hostName).Result == Result::Success);
+
+ if (!ok)
+ {
+ // If failed: inform the user and wait a bit
+ ROS_ERROR("[VICON DATA PUBLISHER] ERROR - connection failed... attempting again in 1 second");
ros::Duration(1.0).sleep();
- } else {
- ROS_INFO("Connected successfully");
+ }
+ else
+ {
+ // If successful: inform the user
+ ROS_INFO("[VICON DATA PUBLISHER] Connected successfully to host");
}
}
- //set data stream parameters
- client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush); //maybe ServerPush instead of ClientPull for less latency
- client.EnableSegmentData();
- client.EnableMarkerData();
- client.EnableUnlabeledMarkerData();
- client.EnableDeviceData();
- // Set the global up axis, such that Z is up
- client.SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up);
+ // SPECIFY THE SETTING OF THE VICON CLIENT
+
+ // Set "stream mode" parameter
+ // > OPTIONS: { ServerPush , ClientPull }
+ // > The "ServerPush" option should have the lowest latency
+ vicon_client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
+
+ // Set what type of data should be provided
+ vicon_client.EnableSegmentData();
+ vicon_client.EnableMarkerData();
+ vicon_client.EnableUnlabeledMarkerData();
+ vicon_client.EnableDeviceData();
+
+ // Set the inertial frame convention such that positive
+ // Z points upwards
+ vicon_client.SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up);
+
+
- while (ros::ok()) {
+ //
+ while (ros::ok())
+ {
// Get a frame
- while (client.GetFrame().Result != Result::Success) {
+ while (vicon_client.GetFrame().Result != Result::Success) {
// Sleep a little so that we don't lumber the CPU with a busy poll
ros::Duration(0.001).sleep();
}
+ // Initilise a "ViconData" struct
+ // > This is defined in the "ViconData.msg" file
ViconData viconData;
// Unlabeled markers, for GUI
- unsigned int unlabeledMarkerCount = client.GetUnlabeledMarkerCount().MarkerCount;
+ unsigned int unlabeledMarkerCount = vicon_client.GetUnlabeledMarkerCount().MarkerCount;
UnlabeledMarker marker;
for(int unlabeledMarkerIndex = 0; unlabeledMarkerIndex < unlabeledMarkerCount; unlabeledMarkerIndex++)
{
Output_GetUnlabeledMarkerGlobalTranslation OutputTranslation =
- client.GetUnlabeledMarkerGlobalTranslation(unlabeledMarkerIndex);
+ vicon_client.GetUnlabeledMarkerGlobalTranslation(unlabeledMarkerIndex);
marker.index = unlabeledMarkerIndex;
marker.x = OutputTranslation.Translation[0]/1000.0f;
@@ -191,21 +174,21 @@ int main(int argc, char* argv[]) {
viconData.markers.push_back(marker);
}
- unsigned int subjectCount = client.GetSubjectCount().SubjectCount;
+ unsigned int subjectCount = vicon_client.GetSubjectCount().SubjectCount;
// //Process the data and publish on topic
for (int index = 0; index < subjectCount; index++) {
- std::string subjectName = client.GetSubjectName(index).SubjectName;
- std::string segmentName = client.GetSegmentName(subjectName, 0).SegmentName;
+ std::string subjectName = vicon_client.GetSubjectName(index).SubjectName;
+ std::string segmentName = vicon_client.GetSegmentName(subjectName, 0).SegmentName;
//continue only if the received frame is for the correct crazyflie
Output_GetSegmentGlobalTranslation outputTranslation =
- client.GetSegmentGlobalTranslation(subjectName, segmentName);
+ vicon_client.GetSegmentGlobalTranslation(subjectName, segmentName);
//ROS_INFO_STREAM("translation occluded: " << outputTranslation.Occluded);
Output_GetSegmentGlobalRotationQuaternion outputRotation =
- client.GetSegmentGlobalRotationQuaternion(subjectName, segmentName);
+ vicon_client.GetSegmentGlobalRotationQuaternion(subjectName, segmentName);
//ROS_INFO_STREAM("translation occluded: " << outputRotation.Occluded);
//calculate position and rotation of Crazyflie
@@ -220,7 +203,7 @@ int main(int argc, char* argv[]) {
double yaw = atan2(2 * (quat_w * quat_z + quat_x * quat_y), 1 - 2 * (quat_y * quat_y + quat_z * quat_z));
//calculate time until frame data was received
- Output_GetLatencyTotal outputLatencyTotal = client.GetLatencyTotal();
+ Output_GetLatencyTotal outputLatencyTotal = vicon_client.GetLatencyTotal();
double totalViconLatency;
if (outputLatencyTotal.Result == Result::Success) {
totalViconLatency = outputLatencyTotal.Total;
@@ -228,6 +211,7 @@ int main(int argc, char* argv[]) {
totalViconLatency = 0;
}
+
//build message
CrazyflieData cfData;
cfData.crazyflieName = subjectName;
@@ -246,13 +230,101 @@ int main(int argc, char* argv[]) {
}
viconDataPublisher.publish(viconData);
}
+ // END OF "while (ros::ok())"
- client.DisableSegmentData();
- client.DisableMarkerData();
- client.DisableUnlabeledMarkerData();
- client.DisableDeviceData();
-
- client.Disconnect();
- #endif
+ // The code only reaches this point if the while loop is
+ // broken, hence disable and diconnect the Vicon client
+ vicon_client.DisableSegmentData();
+ vicon_client.DisableMarkerData();
+ vicon_client.DisableUnlabeledMarkerData();
+ vicon_client.DisableDeviceData();
+ vicon_client.Disconnect();
+#endif
}
+
+
+
+
+
+void testFakeData(ros::Publisher viconDataPublisher)
+{
+ // Test faking data part
+ float f = 0;
+ int i = 0;
+
+ ROS_INFO("[VICON DATA PUBLISHER] TESTING_FAKE_DATA");
+ while(ros::ok())
+ {
+ if(i % 1000 == 0)
+ {
+ ROS_INFO("iteration #%d",i);
+ }
+
+ // Testing piece of code
+ ViconData viconData;
+ UnlabeledMarker marker;
+
+ marker.index = 0;
+ marker.x = f;
+ marker.y = 0;
+ marker.z = 0;
+
+ viconData.markers.push_back(marker);
+
+
+ marker.index = 1;
+ marker.x = 0;
+ marker.y = f;
+ marker.z = 0;
+
+ viconData.markers.push_back(marker);
+
+ if(i > 50 && i < 100)
+ {
+ marker.index = 2;
+ marker.x = f;
+ marker.y = f;
+ marker.z = 0;
+ viconData.markers.push_back(marker);
+ }
+
+ ros::Duration(0.1).sleep();
+ f += 10/1000.0f;
+ i++;
+ // TODO: Fake CF data
+ CrazyflieData crazyfly;
+
+ crazyfly.occluded = false;
+
+ crazyfly.crazyflieName = "CF1";
+ crazyfly.x = 0;
+ crazyfly.y = 0;
+ crazyfly.z = 0;
+ crazyfly.yaw = 3.14159 * f;
+ viconData.crazyflies.push_back(crazyfly);
+
+ crazyfly.crazyflieName = "CF2";
+ crazyfly.x = 1;
+ crazyfly.y = 1;
+ crazyfly.z = 0;
+ crazyfly.yaw = -3.14159 * f;
+ viconData.crazyflies.push_back(crazyfly);
+
+ crazyfly.crazyflieName = "CF3";
+ crazyfly.x = 1;
+ crazyfly.y = -1;
+ crazyfly.z = 0;
+ crazyfly.yaw = -3.14159 * f;
+
+
+ if(i > 50 && i < 200)
+ {
+ crazyfly.occluded = true;
+ }
+
+ viconData.crazyflies.push_back(crazyfly);
+
+ viconDataPublisher.publish(viconData); // testing data
+ }
+}
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/srv/CMCommand.srv b/dfall_ws/src/dfall_pkg/srv/CMCommand.srv
similarity index 100%
rename from pps_ws/src/d_fall_pps/srv/CMCommand.srv
rename to dfall_ws/src/dfall_pkg/srv/CMCommand.srv
diff --git a/pps_ws/src/d_fall_pps/srv/CMQuery.srv b/dfall_ws/src/dfall_pkg/srv/CMQuery.srv
similarity index 100%
rename from pps_ws/src/d_fall_pps/srv/CMQuery.srv
rename to dfall_ws/src/dfall_pkg/srv/CMQuery.srv
diff --git a/pps_ws/src/d_fall_pps/srv/CMRead.srv b/dfall_ws/src/dfall_pkg/srv/CMRead.srv
similarity index 100%
rename from pps_ws/src/d_fall_pps/srv/CMRead.srv
rename to dfall_ws/src/dfall_pkg/srv/CMRead.srv
diff --git a/pps_ws/src/d_fall_pps/srv/CMUpdate.srv b/dfall_ws/src/dfall_pkg/srv/CMUpdate.srv
similarity index 100%
rename from pps_ws/src/d_fall_pps/srv/CMUpdate.srv
rename to dfall_ws/src/dfall_pkg/srv/CMUpdate.srv
diff --git a/pps_ws/src/d_fall_pps/srv/Controller.srv b/dfall_ws/src/dfall_pkg/srv/Controller.srv
similarity index 100%
rename from pps_ws/src/d_fall_pps/srv/Controller.srv
rename to dfall_ws/src/dfall_pkg/srv/Controller.srv
diff --git a/dfall_ws/src/dfall_pkg/srv/GetSetpointService.srv b/dfall_ws/src/dfall_pkg/srv/GetSetpointService.srv
new file mode 100644
index 0000000000000000000000000000000000000000..aea949a642b048e83c1e1cfa4d3ad055cacafe25
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/srv/GetSetpointService.srv
@@ -0,0 +1,3 @@
+uint32 data
+---
+SetpointWithHeader setpointWithHeader
diff --git a/dfall_ws/src/dfall_pkg/srv/IntIntService.srv b/dfall_ws/src/dfall_pkg/srv/IntIntService.srv
new file mode 100644
index 0000000000000000000000000000000000000000..641afd72aba59030843ef0fe0eec48f0f0dc74e7
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/srv/IntIntService.srv
@@ -0,0 +1,3 @@
+uint32 data
+---
+uint32 data
diff --git a/dfall_ws/src/dfall_pkg/srv/LoadYamlFromFilename.srv b/dfall_ws/src/dfall_pkg/srv/LoadYamlFromFilename.srv
new file mode 100644
index 0000000000000000000000000000000000000000..c3f841a95dd3a56131ad8f98aae09857281498db
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/srv/LoadYamlFromFilename.srv
@@ -0,0 +1,3 @@
+StringWithHeader stringWithHeader
+---
+uint32 data
diff --git a/hardware_designs/Connector_3point_v03.STL b/hardware_designs/Connector_3point_v03.STL
new file mode 100644
index 0000000000000000000000000000000000000000..61cdf08dd774b85efdcd39d6676689a17f29cada
Binary files /dev/null and b/hardware_designs/Connector_3point_v03.STL differ
diff --git a/hardware_designs/Load_PM_20mmBox_v01.STL b/hardware_designs/Load_PM_20mmBox_v01.STL
new file mode 100644
index 0000000000000000000000000000000000000000..373d1557e6b125a1d25692f0210d8151240e745e
Binary files /dev/null and b/hardware_designs/Load_PM_20mmBox_v01.STL differ
diff --git a/hardware_designs/MarkerMount_v17.STL b/hardware_designs/MarkerMount_v17.STL
new file mode 100644
index 0000000000000000000000000000000000000000..c83a3b42b03fd49246f18671498d4fac81076acf
Binary files /dev/null and b/hardware_designs/MarkerMount_v17.STL differ
diff --git a/install/pps_install.sh b/install/dfall_install.sh
similarity index 79%
rename from install/pps_install.sh
rename to install/dfall_install.sh
index 233bd38753fd31ea1935d849af0d2d8a52a85561..daba24eed8d7c9b2b99580b28c65938f02bafb5a 100755
--- a/install/pps_install.sh
+++ b/install/dfall_install.sh
@@ -24,8 +24,8 @@ rosdep update
#untar catkin workspace
#needs to run after ros installation because of symbolic link to CMakeLists.txt
-mkdir -p ~/pps_ws/src
-tar -xf package.tar.gz -C ~/pps_ws/src
+mkdir -p ~/dfall_ws/src
+tar -xf package.tar.gz -C ~/dfall_ws/src
#environment setup
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
@@ -39,10 +39,10 @@ sudo cp ./99-crazyflie.rules /etc/udev/rules.d
sudo cp ./99-crazyradio.rules /etc/udev/rules.d
#build workspace
-cd ~/pps_ws
+cd ~/dfall_ws
catkin_make -j4
-echo "source ~/pps_ws/devel/setup.bash" >> ~/.bashrc
-source ~/pps_ws/devel/setup.bash
-echo "source ~/pps_ws/src/d_fall_pps/launch/Config.sh" >> ~/.bashrc
-source ~/pps_ws/src/d_fall_pps/launch/Config.sh
+echo "source ~/dfall_ws/devel/setup.bash" >> ~/.bashrc
+source ~/dfall_ws/devel/setup.bash
+echo "source ~/dfall_ws/src/dfall_pkg/launch/Config.sh" >> ~/.bashrc
+source ~/dfall_ws/src/dfall_pkg/launch/Config.sh
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user
deleted file mode 100755
index c3bd5196277cff69c0be75d8070cd6cb7bc53467..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user
+++ /dev/null
@@ -1,271 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 3.5.1, 2017-12-05T07:54:15. -->
-<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-CrazyFlyGUI-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-CrazyFlyGUI-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">CrazyFlyGUI</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/CrazyFlyGUI/CrazyFlyGUI.pro</value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">CrazyFlyGUI.pro</value>
- <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
- <value type="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/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user.7257614 b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user.7257614
deleted file mode 100755
index ebe2fd318daacb38ff1ff2efeefde05df86d619b..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user.7257614
+++ /dev/null
@@ -1,336 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.2, 2017-08-15T15:30:19. -->
-<qtcreator>
- <data>
- <variable>EnvironmentId</variable>
- <value type="QByteArray">{72576140-2426-4e8d-b4f8-00ed8021ee7f}</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.ActiveTarget</variable>
- <value type="int">0</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.EditorSettings</variable>
- <valuemap type="QVariantMap">
- <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
- <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
- <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
- <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
- <value type="QString" key="language">Cpp</value>
- <valuemap type="QVariantMap" key="value">
- <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
- </valuemap>
- </valuemap>
- <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
- <value type="QString" key="language">QmlJS</value>
- <valuemap type="QVariantMap" key="value">
- <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
- </valuemap>
- </valuemap>
- <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
- <value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
- <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
- <value type="int" key="EditorConfiguration.IndentSize">4</value>
- <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
- <value type="int" key="EditorConfiguration.MarginColumn">80</value>
- <value type="bool" key="EditorConfiguration.MouseHiding">true</value>
- <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
- <value type="int" key="EditorConfiguration.PaddingMode">1</value>
- <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
- <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
- <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
- <value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
- <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
- <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
- <value type="int" key="EditorConfiguration.TabSize">8</value>
- <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
- <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
- <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
- <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
- <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
- <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
- </valuemap>
- </data>
- <data>
- <variable>ProjectExplorer.Project.PluginSettings</variable>
- <valuemap type="QVariantMap"/>
- </data>
- <data>
- <variable>ProjectExplorer.Project.Target.0</variable>
- <valuemap type="QVariantMap">
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop Qt 5.7.0 GCC 64bit</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop Qt 5.7.0 GCC 64bit</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">qt.57.gcc_64_kit</value>
- <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
- <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
- <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-CrazyFlyGUI-Desktop_Qt_5_7_0_GCC_64bit-Debug</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
- <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
- <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
- <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Debug</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
- <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">2</value>
- <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_gui/GUI_Qt/build-CrazyFlyGUI-Desktop_Qt_5_7_0_GCC_64bit-Release</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
- <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
- <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
- <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
- <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
- <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.2">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_gui/GUI_Qt/build-CrazyFlyGUI-Desktop_Qt_5_7_0_GCC_64bit-Profile</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
- <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">true</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
- <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
- <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Profile</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
- <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
- <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">3</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
- <value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
- <value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
- <value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
- <value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
- <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
- <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
- <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
- <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
- <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
- <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
- <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
- <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
- <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
- <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
- <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
- <value type="int">0</value>
- <value type="int">1</value>
- <value type="int">2</value>
- <value type="int">3</value>
- <value type="int">4</value>
- <value type="int">5</value>
- <value type="int">6</value>
- <value type="int">7</value>
- <value type="int">8</value>
- <value type="int">9</value>
- <value type="int">10</value>
- <value type="int">11</value>
- <value type="int">12</value>
- <value type="int">13</value>
- <value type="int">14</value>
- </valuelist>
- <value type="int" key="PE.EnvironmentAspect.Base">2</value>
- <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CrazyFlyGUI</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">CrazyFlyGUI2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro</value>
- <value type="bool" key="QmakeProjectManager.QmakeRunConfiguration.UseLibrarySearchPath">true</value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">CrazyFlyGUI.pro</value>
- <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory.default">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-CrazyFlyGUI-Desktop_Qt_5_7_0_GCC_64bit-Debug</value>
- <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
- <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
- <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
- <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
- <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
- <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
- </valuemap>
- </data>
- <data>
- <variable>ProjectExplorer.Project.TargetCount</variable>
- <value type="int">1</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
- <value type="int">18</value>
- </data>
- <data>
- <variable>Version</variable>
- <value type="int">18</value>
- </data>
-</qtcreator>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/defaultcontrollertab.ui b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/defaultcontrollertab.ui
deleted file mode 100644
index 5c0dc3644bd50811dce99ec132421a9750f7b410..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/defaultcontrollertab.ui
+++ /dev/null
@@ -1,862 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>DefaultControllerTab</class>
- <widget class="QWidget" name="DefaultControllerTab">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>1545</width>
- <height>632</height>
- </rect>
- </property>
- <property name="font">
- <font>
- <pointsize>16</pointsize>
- </font>
- </property>
- <property name="windowTitle">
- <string>Form</string>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="2" column="10">
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="5" column="2">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_row_z">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>z [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="4" column="3">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="7" 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="2" column="1">
- <widget class="QLineEdit" name="lineEdit_error_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_error_title">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Error</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="3">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="2" column="4">
- <spacer name="horizontalSpacer_2">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeType">
- <enum>QSizePolicy::Fixed</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="4" column="7">
- <layout class="QHBoxLayout" name="horizontalLayout_2">
- <item>
- <widget class="QPushButton" name="z_increment_minus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>-</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>140</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="z_increment_plus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>+</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_row_y">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>y [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_row_x">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>x [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="7">
- <layout class="QHBoxLayout" name="horizontalLayout_3">
- <item>
- <widget class="QPushButton" name="y_increment_minus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>-</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>140</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="y_increment_plus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>+</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="3" column="3">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="lineEdit_error_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="5" column="7">
- <layout class="QHBoxLayout" name="horizontalLayout">
- <item>
- <widget class="QPushButton" name="yaw_increment_minus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>-</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>140</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="yaw_increment_plus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>+</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="0" column="7">
- <widget class="QLabel" name="label_increment_title">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Increment</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QLabel" name="label_new_title_line2">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Setpoint</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLabel" name="label_current_title_line2">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <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="lineEdit_setpoint_current_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="lineEdit_error_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QLineEdit" name="lineEdit_error_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QLabel" name="label_row_yaw">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>yaw [deg]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLabel" name="label_error_title_line2">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>(meas-ref)</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="7">
- <layout class="QHBoxLayout" name="horizontalLayout_4">
- <item>
- <widget class="QPushButton" name="x_increment_minus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>-</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>140</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="x_increment_plus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>+</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_current_title">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Current</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="7">
- <widget class="QLabel" name="label_increment_title_line2">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Setpoint</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="5" column="3">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QLabel" name="label_new_title">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>New</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="6" column="2">
- <widget class="QPushButton" name="default_setpoint_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Default</string>
- </property>
- </widget>
- </item>
- <item row="6" column="3">
- <widget class="QPushButton" name="set_setpoint_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Set New</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <resources/>
- <connections/>
-</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
deleted file mode 100644
index 9c03449ca36a55f53b5c0c72da94c6356d85eb0c..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
+++ /dev/null
@@ -1,215 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>MainWindow</class>
- <widget class="QMainWindow" name="MainWindow">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>1862</width>
- <height>839</height>
- </rect>
- </property>
- <property name="windowTitle">
- <string>MainWindow</string>
- </property>
- <widget class="QWidget" name="centralWidget">
- <layout class="QGridLayout" name="gridLayout">
- <item row="0" column="0">
- <layout class="QHBoxLayout" name="horizontalLayout_2">
- <property name="spacing">
- <number>6</number>
- </property>
- <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="Coordinator" name="customWidget_coordinator" native="true">
- <property name="minimumSize">
- <size>
- <width>400</width>
- <height>0</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>680</width>
- <height>16777215</height>
- </size>
- </property>
- </widget>
- </item>
- <item>
- <layout class="QVBoxLayout" name="verticalLayout_2">
- <property name="spacing">
- <number>6</number>
- </property>
- <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="TopBanner" name="customWidget_topBanner" native="true">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>120</height>
- </size>
- </property>
- </widget>
- </item>
- <item>
- <widget class="ConnectStartStopBar" name="customWidget_connectStartStopBar" native="true">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>120</height>
- </size>
- </property>
- </widget>
- </item>
- <item>
- <layout class="QHBoxLayout" name="horizontalLayout">
- <property name="spacing">
- <number>6</number>
- </property>
- <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="ControllerTabs" name="customWidget_controller_tabs" native="true">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item>
- <widget class="EnableControllerLoadYamlBar" name="customWidget_enableControllerLoadYamlBar" native="true">
- <property name="enabled">
- <bool>true</bool>
- </property>
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>200</width>
- <height>0</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>500</width>
- <height>16777215</height>
- </size>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- </layout>
- </item>
- </layout>
- </item>
- </layout>
- </widget>
- <widget class="QMenuBar" name="menuBar">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>1862</width>
- <height>25</height>
- </rect>
- </property>
- <widget class="QMenu" name="menuFile">
- <property name="title">
- <string>File</string>
- </property>
- <addaction name="actionShowHide_Coordinator"/>
- </widget>
- <addaction name="menuFile"/>
- </widget>
- <widget class="QToolBar" name="mainToolBar">
- <attribute name="toolBarArea">
- <enum>TopToolBarArea</enum>
- </attribute>
- <attribute name="toolBarBreak">
- <bool>false</bool>
- </attribute>
- </widget>
- <widget class="QStatusBar" name="statusBar"/>
- <action name="actionShowHide_Coordinator">
- <property name="text">
- <string>Hide Coordinator</string>
- </property>
- </action>
- </widget>
- <layoutdefault spacing="6" margin="11"/>
- <customwidgets>
- <customwidget>
- <class>TopBanner</class>
- <extends>QWidget</extends>
- <header location="global">topbanner.h</header>
- <container>1</container>
- </customwidget>
- <customwidget>
- <class>ConnectStartStopBar</class>
- <extends>QWidget</extends>
- <header location="global">connectstartstopbar.h</header>
- <container>1</container>
- </customwidget>
- <customwidget>
- <class>EnableControllerLoadYamlBar</class>
- <extends>QWidget</extends>
- <header>enablecontrollerloadyamlbar.h</header>
- <container>1</container>
- </customwidget>
- <customwidget>
- <class>ControllerTabs</class>
- <extends>QWidget</extends>
- <header>controllertabs.h</header>
- <container>1</container>
- </customwidget>
- <customwidget>
- <class>Coordinator</class>
- <extends>QWidget</extends>
- <header>coordinator.h</header>
- <container>1</container>
- </customwidget>
- </customwidgets>
- <resources/>
- <connections/>
-</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui
deleted file mode 100644
index 0b90bdd14ad44cacf728ffbca31d7b90981bfe13..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui
+++ /dev/null
@@ -1,28 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>PickerControllerTab</class>
- <widget class="QWidget" name="PickerControllerTab">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>400</width>
- <height>300</height>
- </rect>
- </property>
- <property name="windowTitle">
- <string>Form</string>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="0" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Picker</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <resources/>
- <connections/>
-</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/studentcontrollertab.ui b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/studentcontrollertab.ui
deleted file mode 100644
index f72ca16647c317a1091bc75299e98cb992b22296..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/studentcontrollertab.ui
+++ /dev/null
@@ -1,710 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>StudentControllerTab</class>
- <widget class="QWidget" name="StudentControllerTab">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>1366</width>
- <height>703</height>
- </rect>
- </property>
- <property name="font">
- <font>
- <pointsize>16</pointsize>
- </font>
- </property>
- <property name="windowTitle">
- <string>Form</string>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <item row="7" column="1">
- <widget class="QLabel" name="label_row_pitch">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>pitch [deg]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="2" column="5">
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="8" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_yaw_2">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</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="4" column="2">
- <widget class="QLineEdit" name="lineEdit_error_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLabel" name="label_row_y">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>y [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="3">
- <widget class="QLineEdit" name="lineEdit_current_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QLabel" name="label_row_z">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>z [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_measured_title_line2">
- <property name="text">
- <string>Position</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QLabel" name="label_row_yaw">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>yaw [deg]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <widget class="QLineEdit" name="lineEdit_error_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="5" column="2">
- <widget class="QLineEdit" name="lineEdit_error_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QLabel" name="label_row_x">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>x [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="8" column="1">
- <widget class="QLabel" name="label_row_roll">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>roll [deg]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_measured_title">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Measured</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="7" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_pitch">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="lineEdit_error_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="2" column="4">
- <widget class="QLineEdit" name="lineEdit_new_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="3" column="3">
- <widget class="QLineEdit" name="lineEdit_current_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="4" column="3">
- <widget class="QLineEdit" name="lineEdit_current_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="5" column="3">
- <widget class="QLineEdit" name="lineEdit_current_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <widget class="QLineEdit" name="lineEdit_new_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="4" column="4">
- <widget class="QLineEdit" name="lineEdit_new_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="5" column="4">
- <widget class="QLineEdit" name="lineEdit_new_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="7" column="3">
- <widget class="QPushButton" name="default_setpoint_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Default</string>
- </property>
- </widget>
- </item>
- <item row="7" column="4">
- <widget class="QPushButton" name="set_setpoint_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Set New</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_error_title">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Error</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLabel" name="label_error_title_line2">
- <property name="text">
- <string>(meas - ref)</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QLabel" name="label_current_title">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Current</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QLabel" name="label_current_title_2">
- <property name="text">
- <string>Setpoint</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="0" column="4">
- <widget class="QLabel" name="label_new_title">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>New</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QLabel" name="label_new_title_line2">
- <property name="text">
- <string>Setpoint</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <resources/>
- <connections/>
-</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h
deleted file mode 100644
index 285a788cf615b237c87579fb06e546b06e7a0e5d..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h
+++ /dev/null
@@ -1,28 +0,0 @@
-#ifndef CONNECTSTARTSTOPBAR_H
-#define CONNECTSTARTSTOPBAR_H
-
-#include <QWidget>
-
-//#include <QGraphicsSvgItem>
-//#include <QSvgRenderer>
-
-namespace Ui {
-class ConnectStartStopBar;
-}
-
-class ConnectStartStopBar : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit ConnectStartStopBar(QWidget *parent = 0);
- ~ConnectStartStopBar();
-
-private slots:
- void on_rf_disconnect_button_clicked();
-
-private:
- Ui::ConnectStartStopBar *ui;
-};
-
-#endif // CONNECTSTARTSTOPBAR_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/controllertabs.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/controllertabs.h
deleted file mode 100644
index 3ce7749754833f1c8d0bae089553dec71bee9520..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/controllertabs.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef CONTROLLERTABS_H
-#define CONTROLLERTABS_H
-
-#include <QWidget>
-
-namespace Ui {
-class ControllerTabs;
-}
-
-class ControllerTabs : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit ControllerTabs(QWidget *parent = 0);
- ~ControllerTabs();
-
-private:
- Ui::ControllerTabs *ui;
-};
-
-#endif // CONTROLLERTABS_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinator.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinator.h
deleted file mode 100644
index 82f3faf87a11a2281c42be76d854556513d6165b..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinator.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef COORDINATOR_H
-#define COORDINATOR_H
-
-#include "coordinatorrow.h"
-
-#include <QWidget>
-#include <QVector>
-#include <regex>
-
-namespace Ui {
-class Coordinator;
-}
-
-class Coordinator : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit Coordinator(QWidget *parent = 0);
- ~Coordinator();
-
-private:
- QVector<CoordinatorRow*> vector_of_coordinatorRows;
-
- void remove_all_entries_from_vector_of_coordinatorRows();
-
-private slots:
- void on_refresh_button_clicked();
-
- void on_delete_button_clicked();
-
- void on_coordinate_all_checkBox_clicked();
-
-private:
- Ui::Coordinator *ui;
-};
-
-#endif // COORDINATOR_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
deleted file mode 100644
index 0eecff4b8f9ec63517b6028755ee37d67f5f82e1..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef DEFAULTCONTROLLERTAB_H
-#define DEFAULTCONTROLLERTAB_H
-
-#include <QWidget>
-
-namespace Ui {
-class DefaultControllerTab;
-}
-
-class DefaultControllerTab : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit DefaultControllerTab(QWidget *parent = 0);
- ~DefaultControllerTab();
-
-private:
- Ui::DefaultControllerTab *ui;
-};
-
-#endif // DEFAULTCONTROLLERTAB_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
deleted file mode 100644
index 147dec5b187d453919a3d286ecf64f96510736da..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
+++ /dev/null
@@ -1,74 +0,0 @@
-#ifndef ENABLECONTROLLERLOADYAMLBAR_H
-#define ENABLECONTROLLERLOADYAMLBAR_H
-
-#include <QWidget>
-
-#ifdef CATKIN_MAKE
-#include <std_msgs/Int32.h>
-#include <std_msgs/Float32.h>
-
-#include <ros/ros.h>
-#include <ros/network.h>
-#include <ros/package.h>
-
-// #include "d_fall_pps/AreaBounds.h"
-// #include "d_fall_pps/CrazyflieContext.h"
-// #include "d_fall_pps/CMQuery.h"
-
-// using namespace d_fall_pps;
-#endif
-
-
-// COMMANDS FOR THE FLYING STATE/CONTROLLER USED
-// The constants that "command" changes in the
-// operation state of this agent. These "commands"
-// are sent from this GUI node to the "PPSClient"
-// node where the command is enacted
-#define CMD_USE_SAFE_CONTROLLER 1
-#define CMD_USE_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
-
-
-namespace Ui {
-class EnableControllerLoadYamlBar;
-}
-
-class EnableControllerLoadYamlBar : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit EnableControllerLoadYamlBar(QWidget *parent = 0);
- ~EnableControllerLoadYamlBar();
-
-private slots:
-
- // ENABLE CONTROLLER BUTTONS ON-CLICK CALLBACK
- void on_enable_safe_button_clicked();
- void on_enable_demo_button_clicked();
- void on_enable_student_button_clicked();
- void on_enable_default_button_clicked();
-
- // LOAD YAML BUTTONS ON-CLICK CALLBACK
- void on_load_yaml_safe_button_clicked();
- void on_load_yaml_demo_button_clicked();
- void on_load_yaml_student_button_clicked();
- void on_load_yaml_default_button_clicked();
-
-private:
- Ui::EnableControllerLoadYamlBar *ui;
-
-#ifdef CATKIN_MAKE
- // --------------------------------------------------- //
- // PRIVATE VARIABLES FOR ROS
-
- // PUBLISHERS AND SUBSRIBERS
- // > For Crazyradio commands based on button clicks
- ros::Publisher commandAllPublisher;
-#endif
-};
-
-#endif // ENABLECONTROLLERLOADYAMLBAR_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
deleted file mode 100644
index c8c83a3acd8727fa57fad5fd7c7e7e21e0f10d7e..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef PICKERCONTROLLERTAB_H
-#define PICKERCONTROLLERTAB_H
-
-#include <QWidget>
-
-namespace Ui {
-class PickerControllerTab;
-}
-
-class PickerControllerTab : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit PickerControllerTab(QWidget *parent = 0);
- ~PickerControllerTab();
-
-private:
- Ui::PickerControllerTab *ui;
-};
-
-#endif // PICKERCONTROLLERTAB_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
deleted file mode 100644
index a56804fd88bfa5e976f452d217fbeb569813feb8..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef STUDENTCONTROLLERTAB_H
-#define STUDENTCONTROLLERTAB_H
-
-#include <QWidget>
-
-namespace Ui {
-class StudentControllerTab;
-}
-
-class StudentControllerTab : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit StudentControllerTab(QWidget *parent = 0);
- ~StudentControllerTab();
-
-private:
- Ui::StudentControllerTab *ui;
-};
-
-#endif // STUDENTCONTROLLERTAB_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/topbanner.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/topbanner.h
deleted file mode 100644
index 684801fb3bf0efc91590f1421715ddda53862294..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/topbanner.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef TOPBANNER_H
-#define TOPBANNER_H
-
-#include <QWidget>
-
-namespace Ui {
-class TopBanner;
-}
-
-class TopBanner : public QWidget
-{
- Q_OBJECT
-
-public:
- explicit TopBanner(QWidget *parent = 0);
- ~TopBanner();
-
-private:
- Ui::TopBanner *ui;
-};
-
-#endif // TOPBANNER_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp
deleted file mode 100644
index a7ec4e9d8fe9430e8e7ffd5d8274a3d68d918db1..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include "connectstartstopbar.h"
-#include "ui_connectstartstopbar.h"
-
-ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
- QWidget(parent),
- ui(new Ui::ConnectStartStopBar)
-{
- ui->setupUi(this);
-
- // SET A FEW PROPERTIES OF THE UI ELEMENTS
- // > Default the battery voltage field to be "blank"
- QString qstr = "-.-- V";
- ui->battery_voltage_lineEdit->setText(qstr);
- // > Red font colour for the battery message label
- //ui->battery_message_label->setStyleSheet("QLabel { color : red; }");
- // > Default the battery message label to be "blank"
- //ui->battery_message_label->setText("");
-
- // SET THE DEFAULT IMAGE FOR THE RF CONNECTION STATUS
- QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
- ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
- ui->rf_status_label->setScaledContents(true);
-
- // SET THE DEFAULT IMAGE FOR THE BATTERY STATUS
- QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
- ui->battery_status_label->setPixmap(battery_unknown_pixmap);
- //ui->battery_status_label->setPixmap(battery_status_unknown_pixmap.scaled(ui->battery_status_label->size(), Qt::KeepAspectRatio, Qt::SmoothTransformation));
- ui->battery_status_label->setScaledContents(true);
-
- // SET THE DEFAULT IMAGE FOR THE FLYING STATE
- QPixmap flying_state_unknown_pixmap(":/images/flying_state_unknown.png");
- ui->flying_state_label->setPixmap(flying_state_unknown_pixmap);
- ui->flying_state_label->setScaledContents(true);
-
-
-
-
-
- // ADD KEYBOARD SHORTCUTS
- // > For "all motors off", press the space bar
- ui->motors_off_button->setShortcut(tr("Space"));
-
-
-}
-
-ConnectStartStopBar::~ConnectStartStopBar()
-{
- delete ui;
-}
-
-void ConnectStartStopBar::on_rf_disconnect_button_clicked()
-{
-
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
deleted file mode 100644
index 9c7e79ab75582c577a45fa13a51ec684fe858d4b..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "controllertabs.h"
-#include "ui_controllertabs.h"
-
-ControllerTabs::ControllerTabs(QWidget *parent) :
- QWidget(parent),
- ui(new Ui::ControllerTabs)
-{
- ui->setupUi(this);
-}
-
-ControllerTabs::~ControllerTabs()
-{
- delete ui;
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinator.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinator.cpp
deleted file mode 100644
index 2a89aaa96d0953f0c308dbf54c70abc570c793fd..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinator.cpp
+++ /dev/null
@@ -1,127 +0,0 @@
-#include "coordinator.h"
-#include "ui_coordinator.h"
-
-Coordinator::Coordinator(QWidget *parent) :
- QWidget(parent),
- ui(new Ui::Coordinator)
-{
- ui->setupUi(this);
-
- ui->verticalLayout_for_coordinatedAgentsScrollArea->setAlignment(Qt::AlignTop);
-}
-
-Coordinator::~Coordinator()
-{
- delete ui;
-}
-
-void Coordinator::on_refresh_button_clicked()
-{
- // DELETE ALL EXISTING ROWS
- remove_all_entries_from_vector_of_coordinatorRows();
-
-
-#ifdef CATKIN_MAKE
- // USE A REGULAR EXPRESSION TO IDENTIFY NODES THAT EXIST
- // > The regular expression we use is: \/agent(\d\d\d)\/PPSClient
- // with the different that the escaped character is \\ instead of \ only
-
- // GET A "V_string" OF ALL THE NODES CURRENTLY IN EXISTENCE
- ros::V_string v_str;
- ros::master::getNodes(v_str);
-
- // ITERATE THROUGH THIS "V_string" OF ALL NODE IN EXISTENCE
- for(int i = 0; i < v_str.size(); i++)
- {
- // TEST THE NAME OF THIS NODE AGAINST THE REGULAR EXPRESSION
- std::string s = v_str[i];
- std::smatch m;
- std::regex e ("\\/agent(\\d\\d\\d)\\/PPSClient");
-
- if(std::regex_search(s, m, e))
- {
- // GET THE MATCHED PART OF THE NODE NAME INTO A LOCAL VARIABLE
- // > Note: we use m[1] because we are interested ONLY in the first match
- // > Thus "found_string" should the the agentID, as a string with zero padding
- std::string found_string = m[1].str();
-
-
- // LET THE USER KNOW THAT WE FOUND A MATCH
- ROS_INFO_STREAM("[Coordinator Row GUI] Found an agent with ID = " << found_string.c_str() );
-
- // CONVERT THE STRING TO AN INTEGER
- int this_agentID = stoi(found_string);
-
- // ADD A ROW TO THE COORDINATOR FOR THE AGENT WITH THIS ID FOUND
- CoordinatorRow *temp_coordinatorRow = new CoordinatorRow(this,this_agentID);
-
- // Check the box if "coordinate all" is checked
- if (ui->coordinate_all_checkBox->isChecked())
- {
- temp_coordinatorRow->setShouldCoordinate(true);
- }
- else
- {
- temp_coordinatorRow->setShouldCoordinate(false);
- }
-
- // Add to the vector of coordinator rows
- vector_of_coordinatorRows.append(temp_coordinatorRow);
-
- ui->coordinated_agents_scrollAreaWidgetContents->layout()->addWidget(temp_coordinatorRow);
- }
- }
-#else
-
-
- for ( int i_agent = 1 ; i_agent < 9 ; i_agent++ )
- {
- //ui->scrollAreaWidgetContents->setLayout(new QVBoxLayout);
-
- CoordinatorRow *temp_coordinatorRow = new CoordinatorRow(this,i_agent);
-
- // Check the box if "coordinate all" is checked
- if (ui->coordinate_all_checkBox->isChecked())
- {
- temp_coordinatorRow->setShouldCoordinate(true);
- }
- else
- {
- temp_coordinatorRow->setShouldCoordinate(false);
- }
-
- // Add to the vector of coordinator rows
- vector_of_coordinatorRows.append(temp_coordinatorRow);
-
- ui->coordinated_agents_scrollAreaWidgetContents->layout()->addWidget(temp_coordinatorRow);
- }
-#endif
-}
-
-void Coordinator::on_delete_button_clicked()
-{
- // Call the function that performs the task requested
- remove_all_entries_from_vector_of_coordinatorRows();
-}
-
-void Coordinator::remove_all_entries_from_vector_of_coordinatorRows()
-{
- // Iterate through and delete all entries
- foreach ( CoordinatorRow* temp_coordinatorRow, vector_of_coordinatorRows) {
- delete( temp_coordinatorRow );
- }
- // Clear the vector
- vector_of_coordinatorRows.clear();
-}
-
-void Coordinator::on_coordinate_all_checkBox_clicked()
-{
- // Get the state of the "coordinate all" is check box
- bool shouldCoordinateAll = ui->coordinate_all_checkBox->isChecked();
-
- // Apply this to all the rows
- for ( int irow = 0 ; irow < vector_of_coordinatorRows.length() ; irow++ )
- {
- vector_of_coordinatorRows[irow]->setShouldCoordinate( shouldCoordinateAll );
- }
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
deleted file mode 100644
index 0a4414f367093816133d3435abfde017dde64b2d..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
+++ /dev/null
@@ -1,737 +0,0 @@
-// 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:
-// Coordinator Row GUI.
-//
-// ----------------------------------------------------------------------------------
-
-
-#include "coordinatorrow.h"
-#include "ui_coordinatorrow.h"
-
-CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
- QWidget(parent),
- ui(new Ui::CoordinatorRow),
- my_agentID(agentID)
-{
- ui->setupUi(this);
-
- // CONVERT THE AGENT ID TO A ZERO PADDED STRING
- // > This is the c++ method:
- //std::ostringstream str_stream;
- //str_stream << std::setw(3) << std::setfill('0') << my_agentID;
- //std::string agentID_as_string(str_stream.str());
- // > This is the Qt method
- //my_agentID_as_string = QString("%1").arg(my_agentID, 3, 10, QChar('0'));
- // For which the syntax is:
- // - Arg1: the number
- // - Arg2: how many 0 you want?
- // - Arg3: The base (10 - decimal, 16 hexadecimal)
- // > Alternate Qt method:
- my_agentID_as_string = QString::number(my_agentID).rightJustified(3, '0');
-
- // CONVERT THE AGENT ID TO A STRING FOR THE BASE NAMESPACE
- QString qstr_ros_base_namespace = "/dfall/agent";
- qstr_ros_base_namespace.append(my_agentID_as_string);
- std::string ros_base_namespace = qstr_ros_base_namespace.toStdString();
-
- // SET THE INITIAL VALUE OF THE PRIVATE VARIABLES FOR THIS CLASS
- // > For keeping track of the current RF Crazyradio state
- my_radio_status = CONNECTED;
- // > For keeping track of the current battery state
- my_battery_state = BATTERY_STATE_NORMAL;
- // > For keeping track of which image is currently displayed
- my_battery_status_label_image_current_index = -999;
- // > For keeping track of the current operating state
- my_flying_state = STATE_MOTORS_OFF;
-
- // FOR BATTERY VOLTAGE LIMITS (THESE SHOULD BE READ IN AS PARAMTERS)
- // > When in a "standby" type of state
- battery_voltage_standby_empty = 3.30f;
- battery_voltage_standby_full = 4.20f;
- // > When in a "flying" type of state
- battery_voltage_flying_empty = 2.60f;
- battery_voltage_flying_full = 3.70f;
-
- // SET THE STARTING RADIO STATUS TO BE: DISCONNECTED
- // > this also updates the image for the "rf_status_label", "battery_voltage_lineEdit", and "battery_status_label"
- setCrazyRadioStatus(DISCONNECTED);
- // SET THE STARTING FLYING STATE STATUS TO BE: MOTORS OFF
- // > this also updates the image for the "flying_state_label"
- setFlyingState(CMD_CRAZYFLY_MOTORS_OFF);
- // SET THE DEFAULT NAME FOR THE SELECTED CONTROLLER
- setControllerEnabled(SAFE_CONTROLLER);
-
-#ifdef CATKIN_MAKE
- //m_rosNodeThread = new rosNodeThread(argc, argv, "coordinatorRowGUI");
- //m_rosNodeThread->init();
-
- //m_ros_namespace = ros::this_node::getNamespace();
-
- //qRegisterMetaType<ptrToMessage>("ptrToMessage");
- //QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
-
- //ros::NodeHandle nodeHandle(m_ros_namespace);
-
- // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
- //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
- //ros::NodeHandle nh_PPSClient("PPSClient");
-
-
- // LET THE USER KNOW WHAT THE BASE NAMESPACE IS
- ROS_INFO_STREAM("[Coordinator Row GUI] using base namespace: " << ros_base_namespace.c_str() << ", for agentID = " << my_agentID);
-
- // DEBUGGING FOR NAMESPACES
- //std::string temp_ros_namespace = ros::this_node::getNamespace();
- //ROS_INFO_STREAM("[Coordinator Row GUI] compared to: ros::this_node::getNamespace() = " << temp_ros_namespace.c_str());
-
- // CREATE A NODE HANDLE TO THE BASE NAMESPACE
- ros::NodeHandle base_nodeHandle(ros_base_namespace);
-
- // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
- ros::NodeHandle dfall_root_nodeHandle("/dfall");
-
- // SUBSCRIBERS AND PUBLISHERS:
- // > For Crazyradio commands based on button clicks
- crazyRadioCommandPublisher = base_nodeHandle.advertise<std_msgs::Int32>("PPSClient/crazyRadioCommand", 1);
- // > For updating the "rf_status_label" picture
- crazyRadioStatusSubscriber = base_nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &CoordinatorRow::crazyRadioStatusCallback, this);
- // > For updating the current battery voltage
- batteryVoltageSubscriber = base_nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &CoordinatorRow::batteryVoltageCallback, this);
- // > For updating the current battery state
- batteryStateSubscriber = base_nodeHandle.subscribe("PPSClient/batteryState", 1, &CoordinatorRow::batteryStateChangedCallback, this);
- // > For Flying state commands based on button clicks
- flyingStateCommandPublisher = base_nodeHandle.advertise<std_msgs::Int32>("PPSClient/Command", 1);
- // > For updating the "flying_state_label" picture
- flyingStateSubscriber = base_nodeHandle.subscribe("PPSClient/flyingState", 1, &CoordinatorRow::flyingStateChangedCallback, this);
- // > For changes in the database that defines {agentID,cfID,flying zone} links
- databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("/my_GUI/DBChanged", 1, &CoordinatorRow::databaseChangedCallback, this);;
- centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
- // > For updating the controller that is currently operating
- controllerUsedSubscriber = base_nodeHandle.subscribe("PPSClient/controllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this);
-
-#endif
-
- // FURTHER INITILIASATIONS NEED TO OCCUR AFTER THE ROS RELATED
- // INITIALISATIONS ARE COMPLETE
- loadCrazyflieContext();
-
- // FOR DEBUGGING:
- //ui->shouldCoordinate_checkBox->setText(my_agentID_as_string);
- //ui->shouldCoordinate_checkBox->setText(QString::fromStdString(base_namespace));
-}
-
-CoordinatorRow::~CoordinatorRow()
-{
- delete ui;
-}
-
-
-
-// PUBLIC METHODS FOR SETTING PROPERTIES
-
-// > Set the state of the checkbox
-void CoordinatorRow::setShouldCoordinate(bool shouldCoordinate)
-{
- ui->shouldCoordinate_checkBox->setChecked( shouldCoordinate );
-}
-
-// > For making the "enable flight" and "disable flight" buttons unavailable
-void CoordinatorRow::disableFlyingStateButtons()
-{
- ui->motors_off_button->setEnabled(true);
- ui->enable_flying_button->setEnabled(false);
- ui->disable_flying_button->setEnabled(false);
-}
-
-// > For making the "enable flight" and "disable flight" buttons available
-void CoordinatorRow::enableFlyingStateButtons()
-{
- ui->motors_off_button->setEnabled(true);
- ui->enable_flying_button->setEnabled(true);
- ui->disable_flying_button->setEnabled(true);
-}
-
-
-
-
-
-#ifdef CATKIN_MAKE
-// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
-
-// > For the Battery Voltage
-void CoordinatorRow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
-{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Crazy Radio Status Callback called for agentID = " << my_agentID);
- setCrazyRadioStatus( msg.data );
-}
-#endif
-
-
-// PRIVATE METHODS FOR SETTING PROPERTIES
-
-void CoordinatorRow::setCrazyRadioStatus(int new_radio_status)
-{
- // add more things whenever the status is changed
- switch(new_radio_status)
- {
- case CONNECTED:
- {
- // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
- my_rf_status_label_mutex.lock();
- //ui->rf_status_label->clear();
- QPixmap rf_connected_pixmap(":/images/rf_connected.png");
- ui->rf_status_label->setPixmap(rf_connected_pixmap);
- ui->rf_status_label->setScaledContents(true);
- //ui->rf_status_label->update();
- my_rf_status_label_mutex.unlock();
- // ENABLE THE REMAINDER OF THE GUI
- my_battery_state_mutex.lock();
- if (my_battery_state == BATTERY_STATE_NORMAL)
- {
- enableFlyingStateButtons();
- }
- my_battery_state_mutex.unlock();
-
- break;
- }
-
- case CONNECTING:
- {
- // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
- my_rf_status_label_mutex.lock();
- //ui->rf_status_label->clear();
- QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
- ui->rf_status_label->setPixmap(rf_connecting_pixmap);
- ui->rf_status_label->setScaledContents(true);
- //ui->rf_status_label->update();
- my_rf_status_label_mutex.unlock();
- break;
- }
-
- case DISCONNECTED:
- {
- // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
- my_rf_status_label_mutex.lock();
- //ui->rf_status_label->clear();
- QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
- ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
- ui->rf_status_label->setScaledContents(true);
- //ui->rf_status_label->update();
- my_rf_status_label_mutex.unlock();
- // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
- QString qstr = "-.-- V";
- my_battery_voltage_lineEdit_mutex.lock();
- ui->battery_voltage_lineEdit->setText(qstr);
- my_battery_voltage_lineEdit_mutex.unlock();
- // SET THE APPROPRIATE IMAGE FOR THE BATTERY STATUS LABEL
- // > Lock the mutex for accessing both "my_battery_status_label_image_current_index"
- // and "ui->battery_status_label"
- my_battery_status_label_mutex.lock();
- if (my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
- {
- ui->battery_status_label->clear();
- QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
- ui->battery_status_label->setPixmap(battery_unknown_pixmap);
- ui->battery_status_label->setScaledContents(true);
- my_battery_status_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
- ui->battery_status_label->update();
- }
- my_battery_status_label_mutex.unlock();
- // DISABLE THE REMAINDER OF THE GUI
- disableFlyingStateButtons();
- break;
- }
-
- default:
- {
- break;
- }
- }
- my_radio_status = new_radio_status;
-}
-
-
-
-#ifdef CATKIN_MAKE
-// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
-
-// > For the Battery Voltage
-void CoordinatorRow::batteryVoltageCallback(const std_msgs::Float32& msg)
-{
- setBatteryVoltageTextAndImage( msg.data );
-}
-
-
-void CoordinatorRow::batteryStateChangedCallback(const std_msgs::Int32& msg)
-{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Battery State Changed Callback called for agentID = " << my_agentID);
- setBatteryState( msg.data );
-}
-#endif
-
-
-
-// PRIVATE METHODS FOR SETTING PROPERTIES
-
-// > For updating the battery state
-void CoordinatorRow::setBatteryState(int new_battery_state)
-{
- // LOCK THE MUTEX FOR THE WHOLE SWITCH CASE STATEMENT
- my_battery_state_mutex.lock();
- // Switch depending the the new battery state provided
- switch(new_battery_state)
- {
- case BATTERY_STATE_LOW:
- {
- // MAKE UNAVAILABLE THE BUTTONS FOR ENABLING AND DISABLING FLIGHT
- disableFlyingStateButtons();
-
- // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
- my_battery_state = BATTERY_STATE_LOW;
- break;
- }
-
- case BATTERY_STATE_NORMAL:
- {
- // MAKE UNAVAILABLE THE BUTTONS FOR ENABLING AND DISABLING FLIGHT
- enableFlyingStateButtons();
-
- // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
- my_battery_state = BATTERY_STATE_NORMAL;
- break;
- }
-
- default:
- break;
- }
- // UNLOCK THE MUTEX
- my_battery_state_mutex.unlock();
-}
-
-// > For the battery voltage label and image
-void CoordinatorRow::setBatteryVoltageTextAndImage(float battery_voltage)
-{
- setBatteryVoltageText( battery_voltage );
- setBatteryVoltageImage( battery_voltage );
-}
-
-// > For updating the battery voltage shown in the UI elements of "battery_voltage_lineEdit"
-void CoordinatorRow::setBatteryVoltageText(float battery_voltage)
-{
- // Lock the mutex
- my_battery_voltage_lineEdit_mutex.lock();
- // Construct the text string
- QString qstr = "";
- qstr.append(QString::number(battery_voltage, 'f', 2));
- qstr.append(" V");
- // Set the text to the battery voltage line edit
- ui->battery_voltage_lineEdit->setText(qstr);
- // Unlock the mutex
- my_battery_voltage_lineEdit_mutex.unlock();
-}
-
-// > For updating the battery voltage shown in the UI elements of "battery_status_label"
-void CoordinatorRow::setBatteryVoltageImage(float battery_voltage)
-{
- // COMPUTE THE BATTERY VOLTAGE AS A PERCENTAGE
- float battery_voltage_percentage = fromVoltageToPercent(battery_voltage);
-
- // CONVERT THE VOLTAGE PERCENTAGE TO AN INDEX OF WHICH BATTERY LEVEL IMAGE TO DISPLAY
- // > Initialise a local variable that will be set in the switch case below
- int new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
- // > Initialise a local variable for the string of which image to use
- QString qstr_new_image = "";
- qstr_new_image.append(":/images/");
- // > Get the value of the "my_battery_state" variable into a local variable
- my_battery_state_mutex.lock();
- int local_copy_of_my_battery_state = my_battery_state;
- my_battery_state_mutex.unlock();
- // > Switch based on the current battery state, first locking the mutex for accessing
- // both "my_battery_status_label_image_current_index" and "ui->battery_status_label"
- my_battery_status_label_mutex.lock();
- switch(local_copy_of_my_battery_state)
- {
- // WHEN THE BATTERY IS IN A LOW STATE
- case BATTERY_STATE_LOW:
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
- qstr_new_image.append("battery_empty.png");
- break;
- }
-
- // WHEN THE BATTERY IS IN A NORMAL STATE
- case BATTERY_STATE_NORMAL:
- {
-
- if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY) && (battery_voltage_percentage <= 0.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_EMPTY) && (battery_voltage_percentage <= 2.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
- qstr_new_image.append("battery_empty.png");
- }
- else if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20) && (battery_voltage_percentage <= 20.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_20) && (battery_voltage_percentage <= 22.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_20;
- qstr_new_image.append("battery_20.png");
- }
- else if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40) && (battery_voltage_percentage <= 40.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_40) && (battery_voltage_percentage <= 42.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_40;
- qstr_new_image.append("battery_40.png");
- }
- else if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60) && (battery_voltage_percentage <= 60.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_60) && (battery_voltage_percentage <= 62.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_60;
- qstr_new_image.append("battery_60.png");
- }
- else if (
- ((my_battery_status_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80) && (battery_voltage_percentage <= 80.0f))
- ||
- ((my_battery_status_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_80) && (battery_voltage_percentage <= 82.0f))
- )
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_80;
- qstr_new_image.append("battery_80.png");
- }
- else
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_FULL;
- qstr_new_image.append("battery_full.png");
- }
- break;
- }
-
- default:
- {
- new_battery_label_image_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
- qstr_new_image.append("battery_unknown.png");
- break;
- }
- }
- // UPDATE THE IMAGE DISPLAYED BASED ON THE "new index"
- // > Only if it is different from the current index
- if (my_battery_status_label_image_current_index != new_battery_label_image_index)
- {
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- ui->battery_status_label->clear();
- QPixmap battery_image_pixmap(qstr_new_image);
- ui->battery_status_label->setPixmap(battery_image_pixmap);
- ui->battery_status_label->setScaledContents(true);
- my_battery_status_label_image_current_index = new_battery_label_image_index;
- ui->battery_status_label->update();
- }
- // Finally unlock the mutex
- my_battery_status_label_mutex.unlock();
-}
-
-
-// > For converting a voltage to a percentage, depending on the current "my_flying_state" value
-float CoordinatorRow::fromVoltageToPercent(float voltage)
-{
- // INITIALISE THE LOCAL VARIABLE FOR THE VOLTAGE WHEN FULL/EMPTY
- float voltage_when_full;
- float voltage_when_empty;
-
- // COMPUTE THE PERCENTAGE DIFFERENTLY DEPENDING ON
- // THE CURRENT FLYING STATE
- // > First lock the mutex before accessing the "my_flying_state" variable
- my_flying_state_mutex.lock();
- if (my_flying_state == STATE_MOTORS_OFF)
- {
- // Voltage limits for a "standby" type of state
- voltage_when_empty = battery_voltage_standby_empty;
- voltage_when_full = battery_voltage_standby_full;
- }
- else
- {
- // Voltage limits for a "flying" type of state
- voltage_when_empty = battery_voltage_flying_empty;
- voltage_when_full = battery_voltage_flying_full;
- }
- // > Unlock the mutex
- my_flying_state_mutex.unlock();
-
- // COMPUTE THE PERCENTAGE
- float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
-
- // CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
- // > This should not happen to often
- if(percentage > 100.0f)
- {
- percentage = 100.0f;
- }
- if(percentage < 0.0f)
- {
- percentage = 0.0f;
- }
-
- // RETURN THE PERCENTAGE
- return percentage;
-}
-
-
-
-
-// RESPONDING TO CHANGES IN THE FLYING STATE
-#ifdef CATKIN_MAKE
-void CoordinatorRow::flyingStateChangedCallback(const std_msgs::Int32& msg)
-{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Flying State Changed Callback called for agentID = " << my_agentID);
- setFlyingState(msg.data);
-}
-#endif
-
-void CoordinatorRow::setFlyingState(int new_flying_state)
-{
- // PUT THE CURRENT STATE INTO THE CLASS VARIABLE
- my_flying_state_mutex.lock();
- my_flying_state = new_flying_state;
- my_flying_state_mutex.unlock();
-
- // UPDATE THE LABEL TO DISPLAY THE FLYING STATE
- switch(new_flying_state)
- {
- case STATE_MOTORS_OFF:
- {
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- QPixmap flying_state_off_pixmap(":/images/flying_state_off.png");
- ui->flying_state_label->setPixmap(flying_state_off_pixmap);
- ui->flying_state_label->setScaledContents(true);
- break;
- }
-
- case STATE_TAKE_OFF:
- {
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- QPixmap flying_state_enabling_pixmap(":/images/flying_state_enabling.png");
- ui->flying_state_label->setPixmap(flying_state_enabling_pixmap);
- ui->flying_state_label->setScaledContents(true);
- break;
- }
-
- case STATE_FLYING:
- {
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- QPixmap flying_state_flying_pixmap(":/images/flying_state_flying.png");
- ui->flying_state_label->setPixmap(flying_state_flying_pixmap);
- ui->flying_state_label->setScaledContents(true);
- break;
- }
-
- case STATE_LAND:
- {
- //qstr.append("Land");
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- QPixmap flying_state_disabling_pixmap(":/images/flying_state_disabling.png");
- ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
- ui->flying_state_label->setScaledContents(true);
- break;
- }
-
- default:
- {
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- QPixmap flying_state_unknown_pixmap(":/images/flying_state_unknown.png");
- ui->flying_state_label->setPixmap(flying_state_unknown_pixmap);
- ui->flying_state_label->setScaledContents(true);
- break;
- }
- }
-}
-
-
-// RESPONDING TO CHANGES IN THE DATABASE
-#ifdef CATKIN_MAKE
-// > For the notification that the database was changes, received on the "DatabaseChangedSubscriber"
-void CoordinatorRow::databaseChangedCallback(const std_msgs::Int32& msg)
-{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Database Changed Callback called for agentID = " << my_agentID);
- loadCrazyflieContext();
-}
-#endif
-
-// > For loading the "context" for this agent, i.e., the {agentID,cfID,flying zone} tuple
-void CoordinatorRow::loadCrazyflieContext()
-{
- QString qstr_crazyflie_name = "";
-#ifdef CATKIN_MAKE
- CMQuery contextCall;
- contextCall.request.studentID = my_agentID;
- //ROS_INFO_STREAM("StudentID:" << my_agentID);
-
- centralManagerDatabaseService.waitForExistence(ros::Duration(-1));
-
- if(centralManagerDatabaseService.call(contextCall))
- {
- my_context = contextCall.response.crazyflieContext;
- ROS_INFO_STREAM("[Coordinator Row GUI] CrazyflieContext:\n" << my_context);
-
- qstr_crazyflie_name.append(QString::fromStdString(my_context.crazyflieName));
- }
- else
- {
- ROS_ERROR_STREAM("[Coordinator Row GUI] Failed to load context for agentID = " << my_agentID);
- }
- // This updating of the radio only needs to be done by the actual agent's node
- //ros::NodeHandle nh("CrazyRadio");
- //nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
-#else
- // Set the Crazyflie Name String to be a question mark
- qstr_crazyflie_name.append("?");
-#endif
-
- // Construct and set the string for the checkbox label
- QString qstr_for_checkbox_label = "ID";
- qstr_for_checkbox_label.append(QString::number(my_agentID));
- qstr_for_checkbox_label.append(", CF");
- qstr_for_checkbox_label.append(qstr_crazyflie_name);
- ui->shouldCoordinate_checkBox->setText(qstr_for_checkbox_label);
-
-}
-
-
-#ifdef CATKIN_MAKE
-// > For the controller currently operating, received on "controllerUsedSubscriber"
-void CoordinatorRow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
-{
- //ROS_INFO_STEAM("[Coordinator Row GUI] Controller Used Changed Callback called for agentID = " << my_agentID);
- setControllerEnabled(msg.data);
-}
-#endif
-
-
-void CoordinatorRow::setControllerEnabled(int new_controller)
-{
- switch(new_controller)
- {
- case SAFE_CONTROLLER:
- {
- ui->controller_enabled_label->setText("Safe");
- break;
- }
- case DEMO_CONTROLLER:
- {
- ui->controller_enabled_label->setText("Demo");
- break;
- }
- case STUDENT_CONTROLLER:
- {
- ui->controller_enabled_label->setText("Student");
- break;
- }
- case MPC_CONTROLLER:
- {
- ui->controller_enabled_label->setText("MPC");
- break;
- }
- case REMOTE_CONTROLLER:
- {
- ui->controller_enabled_label->setText("Remote");
- break;
- }
- case TUNING_CONTROLLER:
- {
- ui->controller_enabled_label->setText("Tuning");
- break;
- }
- default:
- {
- ui->controller_enabled_label->setText("Unknown");
- break;
- }
- }
-}
-
-
-
-
-// ------------------------------------------------------------------- //
-// # RF Crazyradio Connect Disconnect
-void CoordinatorRow::on_rf_connect_button_clicked()
-{
-#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
- msg.data = CMD_RECONNECT;
- this->crazyRadioCommandPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Command to RF reconnect published");
-#endif
-}
-
-void CoordinatorRow::on_rf_disconnect_button_clicked()
-{
-#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
- msg.data = CMD_DISCONNECT;
- this->crazyRadioCommandPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Command to RF disconnect published");
-#endif
-}
-
-// ------------------------------------------------------------------- //
-// # Take off, land, motors off
-void CoordinatorRow::on_enable_flying_button_clicked()
-{
-#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
- msg.data = CMD_CRAZYFLY_TAKE_OFF;
- this->flyingStateCommandPublisher.publish(msg);
-#endif
-}
-
-void CoordinatorRow::on_disable_flying_button_clicked()
-{
-#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
- msg.data = CMD_CRAZYFLY_LAND;
- this->flyingStateCommandPublisher.publish(msg);
-#endif
-}
-
-void CoordinatorRow::on_motors_off_button_clicked()
-{
-#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
- msg.data = CMD_CRAZYFLY_MOTORS_OFF;
- this->flyingStateCommandPublisher.publish(msg);
-#endif
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
deleted file mode 100644
index f35ee39e57f15b915207ea93b32b6cfa88784de9..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "defaultcontrollertab.h"
-#include "ui_defaultcontrollertab.h"
-
-DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
- QWidget(parent),
- ui(new Ui::DefaultControllerTab)
-{
- ui->setupUi(this);
-}
-
-DefaultControllerTab::~DefaultControllerTab()
-{
- delete ui;
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
deleted file mode 100644
index e844d06067352607dbd55ec3bb01315f3d5d50b7..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-#include "enablecontrollerloadyamlbar.h"
-#include "ui_enablecontrollerloadyamlbar.h"
-
-EnableControllerLoadYamlBar::EnableControllerLoadYamlBar(QWidget *parent) :
- QWidget(parent),
- ui(new Ui::EnableControllerLoadYamlBar)
-{
- ui->setupUi(this);
-
-
-#ifdef CATKIN_MAKE
- //ros::init();
- // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
- ros::NodeHandle dfall_root_nodeHandle("/dfall");
-
- commandAllPublisher = dfall_root_nodeHandle.advertise<std_msgs::Int32>("/my_GUI/commandAllAgents", 1);
-#endif
-
-}
-
-EnableControllerLoadYamlBar::~EnableControllerLoadYamlBar()
-{
- delete ui;
-}
-
-
-
-
-// ENABLE CONTROLLER BUTTONS ON-CLICK CALLBACK
-
-void EnableControllerLoadYamlBar::on_enable_safe_button_clicked()
-{
-#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
- msg.data = CMD_USE_SAFE_CONTROLLER;
- this->commandAllPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Enable Safe Controller");
-#endif
-}
-
-void EnableControllerLoadYamlBar::on_enable_demo_button_clicked()
-{
-#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
- msg.data = CMD_USE_DEMO_CONTROLLER;
- this->commandAllPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Enable Demo Controller");
-#endif
-}
-
-void EnableControllerLoadYamlBar::on_enable_student_button_clicked()
-{
-#ifdef CATKIN_MAKE
- std_msgs::Int32 msg;
- msg.data = CMD_USE_STUDENT_CONTROLLER;
- this->commandAllPublisher.publish(msg);
- ROS_INFO("[FLYING AGENT GUI] Enable Student Controller");
-#endif
-}
-
-void EnableControllerLoadYamlBar::on_enable_default_button_clicked()
-{
-
-}
-
-
-// LOAD YAML BUTTONS ON-CLICK CALLBACK
-
-void EnableControllerLoadYamlBar::on_load_yaml_safe_button_clicked()
-{
-
-}
-
-void EnableControllerLoadYamlBar::on_load_yaml_demo_button_clicked()
-{
-
-}
-
-void EnableControllerLoadYamlBar::on_load_yaml_student_button_clicked()
-{
-
-}
-
-void EnableControllerLoadYamlBar::on_load_yaml_default_button_clicked()
-{
-
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
deleted file mode 100644
index f71f7205abc9fb93d127874f46727d307f627459..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "pickercontrollertab.h"
-#include "ui_pickercontrollertab.h"
-
-PickerControllerTab::PickerControllerTab(QWidget *parent) :
- QWidget(parent),
- ui(new Ui::PickerControllerTab)
-{
- ui->setupUi(this);
-}
-
-PickerControllerTab::~PickerControllerTab()
-{
- delete ui;
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
deleted file mode 100644
index a47ccd3bba935d80816818980ef826ef63829aaa..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "studentcontrollertab.h"
-#include "ui_studentcontrollertab.h"
-
-StudentControllerTab::StudentControllerTab(QWidget *parent) :
- QWidget(parent),
- ui(new Ui::StudentControllerTab)
-{
- ui->setupUi(this);
-}
-
-StudentControllerTab::~StudentControllerTab()
-{
- delete ui;
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/topbanner.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
deleted file mode 100644
index 4c71863d655ad45dbeb0b04fe641ac24fc8bd7a7..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#include "topbanner.h"
-#include "ui_topbanner.h"
-
-TopBanner::TopBanner(QWidget *parent) :
- QWidget(parent),
- ui(new Ui::TopBanner)
-{
- ui->setupUi(this);
-}
-
-TopBanner::~TopBanner()
-{
- delete ui;
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_20.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_20.png
deleted file mode 100644
index cc7ae62ab7c662f7cf3e098e713232d2d6e0ae14..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_20.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_40.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_40.png
deleted file mode 100644
index cea5ab35b9605910112dd8a7e2eda371c430894b..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_40.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_60.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_60.png
deleted file mode 100644
index 1f75257c3c1f44fc10f318274741942916b47b70..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_60.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_80.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_80.png
deleted file mode 100644
index 8224e7e1e2ed8e251f37f802b14ce4ebd9027f35..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_80.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_empty.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_empty.png
deleted file mode 100644
index 0bf35549aab7c7c0247cebd69a176d68ea67387c..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_empty.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_full.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_full.png
deleted file mode 100644
index 495383be5e346e624d7a92670fcf9ddb053f99ef..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_full.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_unknown.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_unknown.png
deleted file mode 100644
index e20348f65a39bac7c0e40a2a4c0b338ef018dc03..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/battery_unknown.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_disabling.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_disabling.png
deleted file mode 100644
index 667c8027f2b902f08f8e81d3120599a5ed151d48..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_disabling.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_enabling.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_enabling.png
deleted file mode 100644
index ef2dc5bffa6d27edbb6c6ff1171cd77a5af5c20a..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_enabling.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_flying.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_flying.png
deleted file mode 100644
index b0b51f3d42edbfad9f875c1faa413391c4f21de9..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_flying.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_off.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_off.png
deleted file mode 100644
index 70669664f07c12ef27a41f5b5fb918aff93da688..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_off.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_unknown.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_unknown.png
deleted file mode 100644
index 5c0c7337a103e5726af53def62e37ad22209c972..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/flying_state_unknown.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/rf_connected.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/rf_connected.png
deleted file mode 100644
index f9f3580113053f71fe86344e6f34a74e98c17b70..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/rf_connected.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/rf_connecting.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/rf_connecting.png
deleted file mode 100644
index 5f1d4ca1b48496ecf1372e1a0f727058bbc24d95..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/rf_connecting.png and /dev/null differ
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/rf_disconnected.png b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/rf_disconnected.png
deleted file mode 100644
index 0e99e624009059328820b317c908d337caf55ddc..0000000000000000000000000000000000000000
Binary files a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/images/rf_disconnected.png and /dev/null differ
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
deleted file mode 100644
index 48d6bda3aa802985938d78d4121a880e5d842dd5..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ /dev/null
@@ -1,433 +0,0 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
-//
-// This file is part of D-FaLL-System.
-//
-// D-FaLL-System is free software: you can redistribute it and/or modify
-// it under the terms of the GNU General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// (at your option) any later version.
-//
-// D-FaLL-System is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU General Public License
-// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
-//
-//
-// ----------------------------------------------------------------------------------
-// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
-// D D F aaa L L S Y Y S T E MM MM
-// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
-// D D F a aa L L S Y S T E M M
-// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
-//
-//
-// DESCRIPTION:
-// Main window of the Student's GUI
-//
-// ----------------------------------------------------------------------------------
-
-
-#ifndef MAINWINDOW_H
-#define MAINWINDOW_H
-
-#include <QMainWindow>
-#include <QShortcut>
-
-#include <std_msgs/Int32.h>
-#include <std_msgs/Float32.h>
-
-#include "rosNodeThread_for_studentGUI.h"
-
-#include "d_fall_pps/CrazyflieContext.h"
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/Setpoint.h"
-#include "d_fall_pps/SetpointV2.h"
-#include "d_fall_pps/ViconSubscribeObjectName.h"
-
-
-// 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
-#define PICKER_CONTROLLER 7
-
-
-// Commands for CrazyRadio
-#define CMD_RECONNECT 0
-#define CMD_DISCONNECT 1
-
-// CrazyRadio states:
-#define CONNECTED 0
-#define CONNECTING 1
-#define DISCONNECTED 2
-
-// The constants that "command" changes in the
-// operation state of this agent. These "commands"
-// are sent from this GUI node to the "PPSClient"
-// node where the command is enacted
-#define CMD_USE_SAFE_CONTROLLER 1
-#define CMD_USE_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_USE_PICKER_CONTROLLER 7
-
-#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
-
-// Battery label image index
-#define BATTERY_LABEL_IMAGE_INDEX_EMPTY 0
-#define BATTERY_LABEL_IMAGE_INDEX_20 1
-#define BATTERY_LABEL_IMAGE_INDEX_40 2
-#define BATTERY_LABEL_IMAGE_INDEX_60 3
-#define BATTERY_LABEL_IMAGE_INDEX_80 4
-#define BATTERY_LABEL_IMAGE_INDEX_FULL 5
-#define BATTERY_LABEL_IMAGE_INDEX_UNKNOWN 6
-
-// For which controller parameters to load
-#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_PICKER_CONTROLLER_AGENT 7
-
-#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
-#define LOAD_YAML_PICKER_CONTROLLER_COORDINATOR 17
-
-
-// FOR WHICH BUTTON WAS PRESSED IN THE PICKER CONTOLLER
-#define PICKER_BUTTON_GOTOSTART 1
-#define PICKER_BUTTON_ATTACH 2
-#define PICKER_BUTTON_PICKUP 3
-#define PICKER_BUTTON_GOTOEND 4
-#define PICKER_BUTTON_PUTDOWN 5
-#define PICKER_BUTTON_SQUAT 6
-#define PICKER_BUTTON_JUMP 7
-
-#define PICKER_BUTTON_1 11
-#define PICKER_BUTTON_2 12
-#define PICKER_BUTTON_3 13
-#define PICKER_BUTTON_4 14
-
-
-// Universal constants
-#define PI 3.141592653589
-
-#define RAD2DEG 180.0/PI
-#define DEG2RAD PI/180.0
-
-namespace Ui {
-class MainWindow;
-}
-
-class MainWindow : public QMainWindow
-{
- Q_OBJECT
-
-public:
- explicit MainWindow(int argc, char **argv, QWidget *parent = 0);
- ~MainWindow();
-
-private slots:
- void updateNewViconData(const ptrToMessage& p_msg);
-
- // # 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();
-
- // # 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();
-
- // # 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();
- void on_load_picker_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_enable_picker_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);
-
- // Interations with the PICKER controller tab
- // > For the buttons
- void on_picker_gotostart_button_clicked();
- void on_picker_attach_button_clicked();
- void on_picker_pickup_button_clicked();
- void on_picker_gotoend_button_clicked();
- void on_picker_putdown_button_clicked();
- void on_picker_squat_button_clicked();
- void on_picker_jump_button_clicked();
- void on_picker_1_button_clicked();
- void on_picker_2_button_clicked();
- void on_picker_3_button_clicked();
- void on_picker_4_button_clicked();
- // > For the sliders
- void on_picker_x_slider_valueChanged(int value);
- void on_picker_y_slider_valueChanged(int value);
- void on_picker_z_slider_valueChanged(int value);
- void on_picker_mass_slider_valueChanged(int value);
- // > For the dial
- void on_picker_yaw_dial_valueChanged(int value);
-
-
-
-
-
-private:
- Ui::MainWindow *ui;
-
- QShortcut* m_close_GUI_shortcut;
-
- rosNodeThread* m_rosNodeThread;
- int m_radio_status;
- float m_battery_voltage;
- int m_battery_level;
-
- std::string m_ros_namespace;
-
- ros::Timer m_timer_yaml_file_for_safe_controller;
- ros::Timer m_timer_yaml_file_for_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;
- ros::Timer m_timer_yaml_file_for_picker_controller;
-
-
- int m_student_id;
- CrazyflieContext m_context;
-
- Setpoint m_safe_setpoint;
- Setpoint m_demo_setpoint;
- Setpoint m_student_setpoint;
- Setpoint m_mpc_setpoint;
- Setpoint m_picker_setpoint;
-
- int m_flying_state;
- QMutex m_flying_state_mutex;
- QMutex voltage_field_mutex;
- QMutex battery_status_label_mutex;
- QMutex rf_status_label_mutex;
- int m_battery_state;
- // BATTERY EMPTY VOLTAGES (THESE SHOULD BE READ IN AS PARAMTERS)
- //const std::vector<float> m_cutoff_voltages {3.1966, 3.2711, 3.3061, 3.3229, 3.3423, 3.3592, 3.3694, 3.385, 3.4006, 3.4044, 3.4228, 3.4228, 3.4301, 3.4445, 3.4531, 3.4677, 3.4705, 3.4712, 3.4756, 3.483, 3.4944, 3.5008, 3.5008, 3.5084, 3.511, 3.5122, 3.5243, 3.5329, 3.5412, 3.5529, 3.5609, 3.5625, 3.5638, 3.5848, 3.6016, 3.6089, 3.6223, 3.628, 3.6299, 3.6436, 3.6649, 3.6878, 3.6983, 3.7171, 3.7231, 3.7464, 3.7664, 3.7938, 3.8008, 3.816, 3.8313, 3.8482, 3.866, 3.8857, 3.8984, 3.9159, 3.9302, 3.9691, 3.997, 4.14 };
- const float battery_voltage_empty_while_flying = 2.80; // in Volts
- const float battery_voltage_empty_while_motors_off = 3.30; // in Volts
- // BATTERY FULL VOLTAGES
- const float battery_voltage_full_while_flying = 3.70; // in Volts
- const float battery_voltage_full_while_motors_off = 4.20; // in Volts
-
- int m_battery_label_image_current_index;
-
- ros::Publisher crazyRadioCommandPublisher;
- ros::Subscriber crazyRadioStatusSubscriber;
- ros::Publisher PPSClientCommandPublisher;
- ros::Subscriber CFBatterySubscriber;
- ros::Subscriber flyingStateSubscriber;
- ros::Subscriber batteryStateSubscriber;
-
- ros::Publisher controllerSetpointPublisher;
- ros::Subscriber safeSetpointSubscriber;
-
- // 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;
-
-
-
- // > For the PICKER CONTROLLER
- ros::Publisher pickerButtonPressedPublisher;
- ros::Publisher pickerZSetpointPublisher;
- ros::Publisher pickerYawSetpointPublisher;
- ros::Publisher pickerMassPublisher;
- ros::Publisher pickerXAdjustmentPublisher;
- ros::Publisher pickerYAdjustmentPublisher;
- ros::Publisher pickerSetpointPublisher;
- ros::Subscriber pickerSetpointSubscriber;
- ros::Subscriber pickerSetpointToGUISubscriber;
-
- ros::Publisher pickerButtonPressedWithSetpointPublisher;
-
- bool shouldSendWithSetpoint_for_pickerButtons = true;
-
-
-
-
- ros::Publisher demoCustomButtonPublisher;
- ros::Publisher studentCustomButtonPublisher;
-
- ros::Subscriber DBChangedSubscriber;
-
-
-
- // > For publishing a message that requests the
- // YAML parameters to be re-loaded from file
- // > The message contents specify which controller
- // the parameters should be re-loaded for
- ros::Publisher requestLoadControllerYamlPublisher;
-
- // Subscriber for locking the load the controller YAML
- // parameters when the Coordintor GUI requests a load
- ros::Subscriber requestLoadControllerYaml_from_my_GUI_Subscriber;
-
-
- ros::Subscriber controllerUsedSubscriber;
-
- ros::ServiceClient centralManager;
-
- // callbacks
- void crazyRadioStatusCallback(const std_msgs::Int32& msg);
- void CFBatteryCallback(const std_msgs::Float32& msg);
- void flyingStateChangedCallback(const std_msgs::Int32& msg);
-
- void safeSetpointCallback(const Setpoint& newSetpoint);
- void demoSetpointCallback(const Setpoint& newSetpoint);
- void studentSetpointCallback(const Setpoint& newSetpoint);
- void mpcSetpointCallback(const Setpoint& newSetpoint);
- void pickerSetpointCallback(const Setpoint& newSetpoint);
-
-
- void remoteDataCallback(const CrazyflieData& objectData);
- void remoteControlSetpointCallback(const CrazyflieData& setpointData);
-
-
- // > For actually sending the button message
- void send_picker_button_clicked_message(int button_index);
- void send_picker_button_clicked_message_with_setpoint(const SetpointV2& setpointV2_to_send);
-
-
- void DBChangedCallback(const std_msgs::Int32& msg);
-
- // # 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 pickerYamlFileTimerCallback(const ros::TimerEvent&);
-
-
-
- void requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg);
- void controllerUsedChangedCallback(const std_msgs::Int32& msg);
- void batteryStateChangedCallback(const std_msgs::Int32& msg);
-
- float fromVoltageToPercent(float voltage);
- void updateBatteryVoltage(float battery_voltage);
- void setCrazyRadioStatus(int radio_status);
- void loadCrazyflieContext();
- void coordinatesToLocal(CrazyflieData& cf);
-
- void initialize_demo_setpoint();
- void initialize_student_setpoint();
- void initialize_mpc_setpoint();
- void initialize_picker_setpoint();
-
-
- void disableGUI();
- void enableGUI();
- void highlightSafeControllerTab();
- void highlightDemoControllerTab();
- void highlightStudentControllerTab();
- void highlightMpcControllerTab();
- void highlightRemoteControllerTab();
- void highlightTuningControllerTab();
- void highlightPickerControllerTab();
-
- bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
- Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
-
-};
-
-#endif // MAINWINDOW_H
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
deleted file mode 100644
index a1822cb3a9ac01b0f437a40ba7ed0e7749bfcfe8..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ /dev/null
@@ -1,2179 +0,0 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
-//
-// This file is part of D-FaLL-System.
-//
-// D-FaLL-System is free software: you can redistribute it and/or modify
-// it under the terms of the GNU General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// (at your option) any later version.
-//
-// D-FaLL-System is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU General Public License
-// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
-//
-//
-// ----------------------------------------------------------------------------------
-// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
-// D D F aaa L L S Y Y S T E MM MM
-// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
-// D D F a aa L L S Y S T E M M
-// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
-//
-//
-// DESCRIPTION:
-// Main window of the Student's GUI
-//
-// ----------------------------------------------------------------------------------
-
-
-#include "MainWindow.h"
-#include "ui_MainWindow.h"
-#include <string>
-
-#include <ros/ros.h>
-#include <ros/network.h>
-#include <ros/package.h>
-
-#include "d_fall_pps/CMQuery.h"
-
-#include "d_fall_pps/ViconData.h"
-
-#include "d_fall_pps/CustomButton.h"
-
-MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
- QMainWindow(parent),
- ui(new Ui::MainWindow),
- m_battery_level(0)
-{
-
- ui->setupUi(this);
-
- m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
- m_rosNodeThread->init();
-
- setCrazyRadioStatus(DISCONNECTED);
-
- m_ros_namespace = ros::this_node::getNamespace();
- ROS_INFO("[Student GUI] node namespace: %s", m_ros_namespace.c_str());
-
- qRegisterMetaType<ptrToMessage>("ptrToMessage");
- QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
-
- ros::NodeHandle nodeHandle(m_ros_namespace);
-
-
- // 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);
-
-
- // > For the PICKER CONTROLLER
- pickerButtonPressedPublisher = nodeHandle.advertise<std_msgs::Int32>("PickerControllerService/ButtonPressed", 1);
- pickerZSetpointPublisher = nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/ZSetpoint", 1);
- pickerYawSetpointPublisher = nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/YawSetpoint", 1);
- pickerMassPublisher = nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/Mass", 1);
- pickerXAdjustmentPublisher = nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/XAdjustment", 1);
- pickerYAdjustmentPublisher = nodeHandle.advertise<std_msgs::Float32>("PickerControllerService/YAdjustment", 1);
- pickerSetpointPublisher = nodeHandle.advertise<Setpoint>("PickerControllerService/Setpoint", 1);
- pickerSetpointSubscriber = nodeHandle.subscribe("PickerControllerService/Setpoint", 1, &MainWindow::pickerSetpointCallback, this);
- pickerSetpointToGUISubscriber = nodeHandle.subscribe("PickerControllerService/SetpointToGUI", 1, &MainWindow::pickerSetpointCallback, this);
-
- pickerButtonPressedWithSetpointPublisher = nodeHandle.advertise<SetpointV2>("PickerControllerService/ButtonPressedWithSetpoint", 1);
-
- // SET ALL SLIDERS AND DIALS TO DEFAULT VALUES
- ui->picker_z_slider->setValue( 40 );
- ui->picker_mass_slider->setValue( 29 );
- ui->picker_yaw_dial->setValue( 0 );
- ui->picker_x_slider->setValue( 0 );
- ui->picker_y_slider->setValue( 0 );
-
- // SET ALL THE FIELDS TO DEFAULT VALUES
- // > For goto start
- ui->picker_gotostart_x->setText("-0.30");
- ui->picker_gotostart_y->setText("+0.00");
- ui->picker_gotostart_z->setText("+0.50");
- // > For attach
- ui->picker_attach_z->setText("+0.38");
- // > For pickup
- ui->picker_pickup_z->setText("+0.60");
- // > For goto end
- ui->picker_gotoend_x->setText("+0.30");
- ui->picker_gotoend_y->setText("+0.00");
- // > For putdown
- ui->picker_putdown_z->setText("+0.39");
- // > For squat
- ui->picker_squat_z->setText("+0.35");
- // > For jump
- ui->picker_jump_z->setText("+0.50");
- // > For the check boxes
- ui->picker_gotostart_checkBox->setChecked(1);
- ui->picker_attach_checkBox->setChecked(1);
- ui->picker_pickup_checkBox->setChecked(1);
- ui->picker_gotoend_checkBox->setChecked(1);
- ui->picker_putdown_checkBox->setChecked(1);
- ui->picker_squat_checkBox->setChecked(0);
- ui->picker_jump_checkBox->setChecked(0);
-
-
-
-
- // subscribers
- crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
-
- CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);
-
- flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
-
- batteryStateSubscriber = nodeHandle.subscribe("PPSClient/batteryState", 1, &MainWindow::batteryStateChangedCallback, this);
-
- controllerUsedSubscriber = nodeHandle.subscribe("PPSClient/controllerUsed", 1, &MainWindow::controllerUsedChangedCallback, this);
-
-
- safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
- DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
-
- ros::NodeHandle my_nodeHandle("~");
- controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
-
-
- // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
- //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
- ros::NodeHandle nh_PPSClient("PPSClient");
- crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
- PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
-
-
- // > For publishing a message that requests the
- // YAML parameters to be re-loaded from file
- // > The message contents specify which controller
- // the parameters should be re-loaded for
- requestLoadControllerYamlPublisher = nh_PPSClient.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
-
-
- // Subscriber for locking the load the controller YAML
- // parameters when the Coordintor GUI requests a load
- requestLoadControllerYaml_from_my_GUI_Subscriber = nodeHandle.subscribe("/my_GUI/requestLoadControllerYaml", 1, &MainWindow::requestLoadControllerYaml_from_my_GUI_Callback, this);
-
- // First get student ID
- if(!nh_PPSClient.getParam("agentID", m_student_id))
- {
- ROS_ERROR("Failed to get agentID");
- }
-
- // Then, Central manager
- centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
- loadCrazyflieContext();
-
-
-
-
- // Load default setpoint from the "SafeController" namespace of the "ParameterService"
- std::vector<float> default_setpoint(4);
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service("ParameterService");
- ros::NodeHandle nodeHandle_for_safeController(nodeHandle_to_own_agent_parameter_service, "SafeController");
-
- if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
- {
- ROS_ERROR_STREAM("The StudentGUI could not find parameter 'defaultSetpoint', as called from main(...)");
- }
-
- // Copy the default setpoint into respective text fields of the GUI
- ui->current_setpoint_x_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();
- highlightSafeControllerTab();
-
- // INITIALISE THE BATTERY STATE AS NORMAL
- m_battery_state = BATTERY_STATE_NORMAL;
-
- // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
- QString qstr = "-.-- V";
- ui->voltage_field->setText(qstr);
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
- ui->battery_status_label->setPixmap(battery_unknown_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
-
- // SET THE IMAGE FOR THE CRAZY RADIO STATUS LABEL
- QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
- ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
- ui->rf_status_label->setScaledContents(true);
-
- // SET THE IMAGE FOR THE FLYING STATE LABEL
- QPixmap flying_state_off_pixmap(":/images/flying_state_off.png");
- ui->flying_state_label->setPixmap(flying_state_off_pixmap);
- ui->flying_state_label->setScaledContents(true);
-
-
-
- //QPixmap battery_80_pixmap(":/images/battery_80.png");
- //m_battery_80_pixmap = battery_80_pixmap.scaled(50,70,Qt::IgnoreAspectRatio);
- // The syntax for "scaled" is (int width, int height, ...)
-
-
- ui->error_label->setStyleSheet("QLabel { color : red; }");
- ui->error_label->clear();
-
- // 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
- m_close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
-
-
- initialize_demo_setpoint();
- initialize_student_setpoint();
- initialize_mpc_setpoint();
- initialize_picker_setpoint();
-}
-
-
-MainWindow::~MainWindow()
-{
- delete ui;
-}
-
-void MainWindow::disableGUI()
-{
- ui->motors_OFF_button->setEnabled(false);
- ui->take_off_button->setEnabled(false);
- ui->land_button->setEnabled(false);
-}
-
-void MainWindow::enableGUI()
-{
- ui->motors_OFF_button->setEnabled(true);
- if(m_battery_state == BATTERY_STATE_NORMAL)
- {
- ui->take_off_button->setEnabled(true);
- ui->land_button->setEnabled(true);
- }
-}
-
-void MainWindow::highlightSafeControllerTab()
-{
- ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
- ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
- 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);
- ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
-}
-void MainWindow::highlightPickerControllerTab()
-{
- 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);
- ui->tabWidget->tabBar()->setTabTextColor(6, Qt::black);
-}
-void MainWindow::highlightDemoControllerTab()
-{
- 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);
- ui->tabWidget->tabBar()->setTabTextColor(6, 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::black);
- ui->tabWidget->tabBar()->setTabTextColor(3, Qt::green);
- ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
- ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
- ui->tabWidget->tabBar()->setTabTextColor(6, 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::black);
- ui->tabWidget->tabBar()->setTabTextColor(4, Qt::green);
- ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
- ui->tabWidget->tabBar()->setTabTextColor(6, 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::black);
- ui->tabWidget->tabBar()->setTabTextColor(5, Qt::green);
- ui->tabWidget->tabBar()->setTabTextColor(6, 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::black);
- ui->tabWidget->tabBar()->setTabTextColor(6, Qt::green);
-}
-
-
-void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
-{
- loadCrazyflieContext();
- ROS_INFO("context reloaded in student_GUI");
-}
-
-void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
-{
- switch(msg.data)
- {
- case SAFE_CONTROLLER:
- highlightSafeControllerTab();
- break;
- case DEMO_CONTROLLER:
- highlightDemoControllerTab();
- break;
- case STUDENT_CONTROLLER:
- highlightStudentControllerTab();
- break;
- case MPC_CONTROLLER:
- highlightMpcControllerTab();
- break;
- case REMOTE_CONTROLLER:
- highlightRemoteControllerTab();
- break;
- case TUNING_CONTROLLER:
- highlightTuningControllerTab();
- break;
- case PICKER_CONTROLLER:
- highlightPickerControllerTab();
- break;
- default:
- break;
- }
-}
-
-void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
-{
- m_safe_setpoint = newSetpoint;
- // here we get the new setpoint, need to update it in GUI
- ui->current_setpoint_x_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::mpcSetpointCallback(const Setpoint& newSetpoint)
-{
- m_mpc_setpoint = newSetpoint;
- // here we get the new setpoint, need to update it in GUI
- 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::pickerSetpointCallback(const Setpoint& newSetpoint)
-{
- m_picker_setpoint = newSetpoint;
- // here we get the new setpoint, need to update it in GUI
- ui->picker_z_slider->setValue( int(newSetpoint.z*100.0) );
- ui->picker_yaw_dial->setValue( int(newSetpoint.yaw * RAD2DEG) );
-
-}
-
-void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
-{
- // PUT THE CURRENT STATE INTO THE CLASS VARIABLE
- m_flying_state_mutex.lock();
- m_flying_state = msg.data;
- m_flying_state_mutex.unlock();
-
- // UPDATE THE LABEL TO DISPLAY THE FLYING STATE
- //QString qstr = "Flying State: ";
- switch(msg.data)
- {
- case STATE_MOTORS_OFF:
- {
- //qstr.append("Motors OFF");
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- ui->flying_state_label->clear();
- QPixmap flying_state_off_pixmap(":/images/flying_state_off.png");
- ui->flying_state_label->setPixmap(flying_state_off_pixmap);
- ui->flying_state_label->setScaledContents(true);
- ui->flying_state_label->update();
- break;
- }
-
- case STATE_TAKE_OFF:
- {
- //qstr.append("Take OFF");
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- ui->flying_state_label->clear();
- QPixmap flying_state_enabling_pixmap(":/images/flying_state_enabling.png");
- ui->flying_state_label->setPixmap(flying_state_enabling_pixmap);
- ui->flying_state_label->setScaledContents(true);
- ui->flying_state_label->update();
- break;
- }
-
- case STATE_FLYING:
- {
- //qstr.append("Flying");
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- ui->flying_state_label->clear();
- QPixmap flying_state_flying_pixmap(":/images/flying_state_flying.png");
- ui->flying_state_label->setPixmap(flying_state_flying_pixmap);
- ui->flying_state_label->setScaledContents(true);
- ui->flying_state_label->update();
- break;
- }
-
- case STATE_LAND:
- {
- //qstr.append("Land");
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- ui->flying_state_label->clear();
- QPixmap flying_state_disabling_pixmap(":/images/flying_state_disabling.png");
- ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
- ui->flying_state_label->setScaledContents(true);
- ui->flying_state_label->update();
- break;
- }
-
- default:
- {
- // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
- ui->flying_state_label->clear();
- QPixmap flying_state_unknown_pixmap(":/images/flying_state_unknown.png");
- ui->flying_state_label->setPixmap(flying_state_unknown_pixmap);
- ui->flying_state_label->setScaledContents(true);
- ui->flying_state_label->update();
- break;
- }
- }
- //ui->flying_state_label->setText(qstr);
-}
-
-void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
-{
- // switch case with unabling buttons motors off, take off, etc... when battery is low
- QString qstr = "";
- switch(msg.data)
- {
- case BATTERY_STATE_LOW:
- {
- // DISABLE THE TAKE OFF AND LAND BUTTONS
- ui->take_off_button->setEnabled(false);
- ui->land_button->setEnabled(false);
- // ui->groupBox_4->setEnabled(false);
-
- // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
- m_battery_state = BATTERY_STATE_LOW;
- break;
- }
-
- case BATTERY_STATE_NORMAL:
- {
- // ui->groupBox_4->setEnabled(true);
- ui->take_off_button->setEnabled(true);
- ui->land_button->setEnabled(true);
-
- // SET THE CLASS VARIABLE FOR TRACKING THE BATTERY STATE
- m_battery_state = BATTERY_STATE_NORMAL;
- break;
- }
-
- default:
- break;
- }
-}
-
-
-void MainWindow::setCrazyRadioStatus(int radio_status)
-{
- // add more things whenever the status is changed
- switch(radio_status)
- {
- case CONNECTED:
- {
- // SET THE APPROPRIATE IMAGE FOR THE RADIOSTATUS LABEL
- rf_status_label_mutex.lock();
- //ui->rf_status_label->clear();
- QPixmap rf_connected_pixmap(":/images/rf_connected.png");
- ui->rf_status_label->setPixmap(rf_connected_pixmap);
- ui->rf_status_label->setScaledContents(true);
- //ui->rf_status_label->update();
- rf_status_label_mutex.unlock();
- // ENABLE THE REMAINDER OF THE GUI
- enableGUI();
- break;
- }
-
- case CONNECTING:
- {
- // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
- rf_status_label_mutex.lock();
- //ui->rf_status_label->clear();
- QPixmap rf_connecting_pixmap(":/images/rf_connecting.png");
- ui->rf_status_label->setPixmap(rf_connecting_pixmap);
- ui->rf_status_label->setScaledContents(true);
- //ui->rf_status_label->update();
- rf_status_label_mutex.unlock();
- break;
- }
-
- case DISCONNECTED:
- {
- // SET THE APPROPRIATE IMAGE FOR THE RADIO STATUS LABEL
- rf_status_label_mutex.lock();
- //ui->rf_status_label->clear();
- QPixmap rf_disconnected_pixmap(":/images/rf_disconnected.png");
- ui->rf_status_label->setPixmap(rf_disconnected_pixmap);
- ui->rf_status_label->setScaledContents(true);
- //ui->rf_status_label->update();
- rf_status_label_mutex.unlock();
- // SET THE BATTERY VOLTAGE FIELD TO BE BLANK
- QString qstr = "-.-- V";
- voltage_field_mutex.lock();
- ui->voltage_field->setText(qstr);
- voltage_field_mutex.unlock();
- // SET THE APPROPRIATE IMAGE FOR THE BATTERY STATUS LABEL
- battery_status_label_mutex.lock();
- if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
- {
- ui->battery_status_label->clear();
- QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
- ui->battery_status_label->setPixmap(battery_unknown_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
- ui->battery_status_label->update();
- }
- battery_status_label_mutex.unlock();
- // DISABLE THE REMAINDER OF THE GUI
- disableGUI();
- break;
- }
-
- default:
- {
- ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
- break;
- }
- }
- this->m_radio_status = radio_status;
-}
-
-
-
-
-float MainWindow::fromVoltageToPercent(float voltage)
-{
- // INITIALISE THE LOCAL VARIABLE FOR THE VOLTAGE WHEN FULL/EMPTY
- float voltage_when_full;
- float voltage_when_empty;
-
- // COMPUTE THE PERCENTAGE DIFFERENTLY DEPENDING ON
- // THE CURRENT FLYING STATE
- m_flying_state_mutex.lock();
- if (m_flying_state == STATE_MOTORS_OFF)
- {
- voltage_when_empty = battery_voltage_empty_while_motors_off;
- voltage_when_full = battery_voltage_full_while_motors_off;
- }
- else
- {
- voltage_when_empty = battery_voltage_empty_while_flying;
- voltage_when_full = battery_voltage_full_while_flying;
- }
- m_flying_state_mutex.unlock();
- //voltage_when_empty = battery_voltage_empty_while_motors_off;
- //voltage_when_full = battery_voltage_full_while_motors_off;
-
- // COMPUTE THE PERCENTAGE
- float percentage = 100.0f * (voltage-voltage_when_empty)/(voltage_when_full-voltage_when_empty);
-
-
- // CLIP THE PERCENTAGE TO BE BETWEEN [0,100]
- // > This should not happen to often
- if(percentage > 100.0f)
- {
- percentage = 100.0f;
- }
- if(percentage < 0.0f)
- {
- percentage = 0.0f;
- }
-
- return percentage;
-}
-
-void MainWindow::updateBatteryVoltage(float battery_voltage)
-{
- // PUT THE VOLTAGE INTO THE CLASS VARIABLES
- m_battery_voltage = battery_voltage;
-
- // UPDATE THE BATTERY VOLTAGE FIELD
- voltage_field_mutex.lock();
- QString qstr = "";
- qstr.append(QString::number(battery_voltage, 'f', 2));
- qstr.append(" V");
- ui->voltage_field->setText(qstr);
- voltage_field_mutex.unlock();
-
- // COMPUTE THE BATTERY VOLTAGE AS A PERCENTAGE
- float battery_voltage_percentage = fromVoltageToPercent(battery_voltage);
-
- //ROS_INFO_STREAM("Battery percentage = " << battery_voltage_percentage );
-
- // UPDATE THE IMAGE DISPLAYED IN THE BATTERY VOLTAGE LABEL IMAGE
- battery_status_label_mutex.lock();
- switch(m_battery_state)
- {
- // WHEN THE BATTERY IS IN A LOW STATE
- case BATTERY_STATE_LOW:
- {
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
- {
- ui->battery_status_label->clear();
- QPixmap battery_empty_pixmap(":/images/battery_empty.png");
- ui->battery_status_label->setPixmap(battery_empty_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
- ui->battery_status_label->update();
- }
- break;
- }
-
- // WHEN THE BATTERY IS IN A NORMAL STATE
- case BATTERY_STATE_NORMAL:
- {
-
- if (
- ((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY) && (battery_voltage_percentage <= 0.0f))
- ||
- ((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_EMPTY) && (battery_voltage_percentage <= 2.0f))
- )
- {
- if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_EMPTY)
- {
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- ui->battery_status_label->clear();
- QPixmap battery_empty_pixmap(":/images/battery_empty.png");
- ui->battery_status_label->setPixmap(battery_empty_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_EMPTY;
- ui->battery_status_label->update();
- }
- }
- else if (
- ((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20) && (battery_voltage_percentage <= 20.0f))
- ||
- ((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_20) && (battery_voltage_percentage <= 22.0f))
- )
- {
- if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_20)
- {
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- ui->battery_status_label->clear();
- QPixmap battery_20_pixmap(":/images/battery_20.png");
- ui->battery_status_label->setPixmap(battery_20_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_20;
- ui->battery_status_label->update();
- }
- }
- else if (
- ((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40) && (battery_voltage_percentage <= 40.0f))
- ||
- ((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_40) && (battery_voltage_percentage <= 42.0f))
- )
- {
- if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_40)
- {
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- ui->battery_status_label->clear();
- QPixmap battery_40_pixmap(":/images/battery_40.png");
- ui->battery_status_label->setPixmap(battery_40_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_40;
- ui->battery_status_label->update();
- }
- }
- else if (
- ((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60) && (battery_voltage_percentage <= 60.0f))
- ||
- ((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_60) && (battery_voltage_percentage <= 62.0f))
- )
- {
- if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_60)
- {
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- ui->battery_status_label->clear();
- QPixmap battery_60_pixmap(":/images/battery_60.png");
- ui->battery_status_label->setPixmap(battery_60_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_60;
- ui->battery_status_label->update();
- }
- }
- else if (
- ((m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80) && (battery_voltage_percentage <= 80.0f))
- ||
- ((m_battery_label_image_current_index == BATTERY_LABEL_IMAGE_INDEX_80) && (battery_voltage_percentage <= 82.0f))
- )
- {
- if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_80)
- {
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- ui->battery_status_label->clear();
- QPixmap battery_80_pixmap(":/images/battery_80.png");
- ui->battery_status_label->setPixmap(battery_80_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_80;
- ui->battery_status_label->update();
- }
- }
- else
- {
- if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_FULL)
- {
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- ui->battery_status_label->clear();
- QPixmap battery_full_pixmap(":/images/battery_full.png");
- ui->battery_status_label->setPixmap(battery_full_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_FULL;
- ui->battery_status_label->update();
- }
- }
- break;
- }
-
- default:
- {
- if (m_battery_label_image_current_index != BATTERY_LABEL_IMAGE_INDEX_UNKNOWN)
- {
- // SET THE IMAGE FOR THE BATTERY STATUS LABEL
- ui->battery_status_label->clear();
- QPixmap battery_unknown_pixmap(":/images/battery_unknown.png");
- ui->battery_status_label->setPixmap(battery_unknown_pixmap);
- ui->battery_status_label->setScaledContents(true);
- m_battery_label_image_current_index = BATTERY_LABEL_IMAGE_INDEX_UNKNOWN;
- ui->battery_status_label->update();
- }
- break;
- }
- }
- battery_status_label_mutex.unlock();
-}
-
-void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)
-{
- updateBatteryVoltage(msg.data);
-}
-
-void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
-{
- ROS_INFO("[Student GUI] Callback CrazyRadioStatus called");
- this->setCrazyRadioStatus(msg.data);
-}
-
-void MainWindow::loadCrazyflieContext()
-{
- CMQuery contextCall;
- contextCall.request.studentID = m_student_id;
- ROS_INFO_STREAM("StudentID:" << m_student_id);
-
- centralManager.waitForExistence(ros::Duration(-1));
-
- if(centralManager.call(contextCall))
- {
- m_context = contextCall.response.crazyflieContext;
- ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
- // we now have the m_context variable with the current context. Put CF Name in label
-
- QString qstr = "StudentID ";
- qstr.append(QString::number(m_student_id));
- qstr.append(" connected to CF ");
- qstr.append(QString::fromStdString(m_context.crazyflieName));
- ui->groupBox->setTitle(qstr);
- }
- else
- {
- ROS_ERROR("Failed to load context");
- }
-
- ros::NodeHandle nh("CrazyRadio");
- nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
-}
-
-void MainWindow::coordinatesToLocal(CrazyflieData& cf)
-{
- AreaBounds area = m_context.localArea;
- float originX = (area.xmin + area.xmax) / 2.0;
- float originY = (area.ymin + area.ymax) / 2.0;
- // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
- float originZ = 0.0;
- // float originZ = (area.zmin + area.zmax) / 2.0;
-
- cf.x -= originX;
- cf.y -= originY;
- cf.z -= originZ;
-}
-
-
-void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
-{
- for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
- {
- CrazyflieData global = *it;
- if(global.crazyflieName == m_context.crazyflieName)
- {
- CrazyflieData local = global;
- coordinatesToLocal(local);
-
- // now we have the local coordinates, put them in the labels
- ui->current_x_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));
-
- ui->current_x_student->setText(QString::number(local.x, 'f', 3));
- ui->current_y_student->setText(QString::number(local.y, 'f', 3));
- ui->current_z_student->setText(QString::number(local.z, 'f', 3));
- ui->current_yaw_student->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
- ui->current_pitch_student->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
- ui->current_roll_student->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
-
- // also update diff
- 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));
-
- ui->diff_x_student->setText(QString::number(m_student_setpoint.x - local.x, 'f', 3));
- ui->diff_y_student->setText(QString::number(m_student_setpoint.y - local.y, 'f', 3));
- ui->diff_z_student->setText(QString::number(m_student_setpoint.z - local.z, 'f', 3));
- ui->diff_yaw_student->setText(QString::number((m_student_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
- }
- }
-}
-
-
-
-// ----------------------------------------------------------------------------------
-// # RF Crazyradio Connect Disconnect
-void MainWindow::on_RF_Connect_button_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_RECONNECT;
- this->crazyRadioCommandPublisher.publish(msg);
- ROS_INFO("command reconnect published");
-}
-
-void MainWindow::on_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;
- msg.data = CMD_CRAZYFLY_TAKE_OFF;
- this->PPSClientCommandPublisher.publish(msg);
-}
-
-void MainWindow::on_land_button_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_CRAZYFLY_LAND;
- this->PPSClientCommandPublisher.publish(msg);
-}
-
-void MainWindow::on_motors_OFF_button_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_CRAZYFLY_MOTORS_OFF;
- this->PPSClientCommandPublisher.publish(msg);
-}
-
-
-
-// ----------------------------------------------------------------------------------
-// # Setpoint
-void MainWindow::on_set_setpoint_button_safe_clicked()
-{
- Setpoint msg_setpoint;
-
- // initialize setpoint to previous one
-
- 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_safe->text().isEmpty())
- msg_setpoint.x = (ui->new_setpoint_x_safe->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_safe->text().isEmpty())
- msg_setpoint.z = (ui->new_setpoint_z_safe->text()).toFloat();
-
- 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))
- {
- ROS_INFO("Corrected setpoint, was out of bounds");
-
- // correct the setpoint given the box size
- msg_setpoint = correctSetpointBox(msg_setpoint, m_context);
- ui->error_label->setText("Setpoint is outside safety box");
- }
- else
- {
- ui->error_label->clear();
- }
-
- this->controllerSetpointPublisher.publish(msg_setpoint);
-
- ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
-}
-
-void MainWindow::initialize_demo_setpoint()
-{
- Setpoint msg_setpoint;
- msg_setpoint.x = 0;
- msg_setpoint.y = 0;
- msg_setpoint.z = 0.4;
- msg_setpoint.yaw = 0;
-
- this->demoSetpointPublisher.publish(msg_setpoint);
-}
-
-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;
-
- this->studentSetpointPublisher.publish(msg_setpoint);
-}
-
-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::initialize_picker_setpoint()
-{
- Setpoint msg_setpoint;
- msg_setpoint.x = 0;
- msg_setpoint.y = 0;
- msg_setpoint.z = 0.4;
- msg_setpoint.yaw = 0;
-
- this->pickerSetpointPublisher.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();
-
- 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_set_setpoint_button_student_clicked()
-{
- 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
- ui->load_safe_yaml_button->setEnabled(false);
-
- // Send a message requesting the parameters from the YAML
- // file to be reloaded for the safe controller
- std_msgs::Int32 msg;
- msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
- this->requestLoadControllerYamlPublisher.publish(msg);
- ROS_INFO("Request load of safe controller YAML published");
-
- // Start a timer which will enable the button in its callback
- // > This is required because the agent node waits some time between
- // re-loading the values from the YAML file and then assigning then
- // to the local variable of the agent.
- // > Thus we use this timer to prevent the user from clicking the
- // button in the GUI repeatedly.
- ros::NodeHandle nodeHandle("~");
- m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
-}
-
-void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
-{
- // Enble the "load safe yaml" button again
- ui->load_safe_yaml_button->setEnabled(true);
-}
-
-
-
-void MainWindow::on_load_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);
-
- // 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 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 remote controller
- std_msgs::Int32 msg;
- msg.data = LOAD_YAML_REMOTE_CONTROLLER_AGENT;
- this->requestLoadControllerYamlPublisher.publish(msg);
- 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
- // 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_remote_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::remoteYamlFileTimerCallback, this, true);
-}
-
-void MainWindow::remoteYamlFileTimerCallback(const ros::TimerEvent&)
-{
- // 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::on_load_picker_yaml_button_clicked()
-{
- // Set the "load picker yaml" button to be disabled
- ui->load_picker_yaml_button->setEnabled(false);
-
- // Send a message requesting the parameters from the YAML
- // file to be reloaded for the picker controller
- std_msgs::Int32 msg;
- msg.data = LOAD_YAML_PICKER_CONTROLLER_AGENT;
- this->requestLoadControllerYamlPublisher.publish(msg);
- ROS_INFO("[STUDENT GUI] Request load of picker 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_picker_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::pickerYamlFileTimerCallback, this, true);
-}
-
-void MainWindow::pickerYamlFileTimerCallback(const ros::TimerEvent&)
-{
- // Enble the "load picker yaml" button again
- ui->load_picker_yaml_button->setEnabled(true);
-}
-
-
-void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg)
-{
- // Extract from the "msg" for which controller the YAML
- // parameters should be loaded
- int controller_to_load_yaml = msg.data;
-
- // Create the "nodeHandle" needed in the switch cases below
- ros::NodeHandle nodeHandle("~");
-
- // Switch between loading for the different controllers
- switch(controller_to_load_yaml)
- {
- case LOAD_YAML_SAFE_CONTROLLER_AGENT:
- case LOAD_YAML_SAFE_CONTROLLER_COORDINATOR:
- // Set the "load safe yaml" button to be disabled
- ui->load_safe_yaml_button->setEnabled(false);
-
- // Start a timer which will enable the button in its callback
- // > This is required because the agent node waits some time between
- // re-loading the values from the YAML file and then assigning then
- // to the local variable of the agent.
- // > Thus we use this timer to prevent the user from clicking the
- // button in the GUI repeatedly.
- m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
-
- break;
-
- case LOAD_YAML_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
- // 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_tuning_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::tuningYamlFileTimerCallback, this, true);
-
- break;
-
- case LOAD_YAML_PICKER_CONTROLLER_AGENT:
- case LOAD_YAML_PICKER_CONTROLLER_COORDINATOR:
- // Set the "load picker yaml" button to be disabled
- ui->load_picker_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_picker_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::pickerYamlFileTimerCallback, this, true);
-
- break;
-
- default:
- ROS_INFO("Unknown 'all controllers to load yaml' command, thus nothing will be disabled");
- break;
- }
-}
-
-
-
-
-
-// # Enable controllers
-void MainWindow::on_enable_safe_controller_clicked()
-{
- std_msgs::Int32 msg;
- 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_enable_student_controller_clicked()
-{
- std_msgs::Int32 msg;
- 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_enable_tuning_controller_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_USE_TUNING_CONTROLLER;
- this->PPSClientCommandPublisher.publish(msg);
-}
-
-void MainWindow::on_enable_picker_controller_clicked()
-{
- std_msgs::Int32 msg;
- msg.data = CMD_USE_PICKER_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->studentCustomButtonPublisher.publish(msg_custom_button);
-
- ROS_INFO("Student button 1 pressed in GUI");
-}
-
-void MainWindow::on_studentButton_2_clicked()
-{
- CustomButton msg_custom_button;
- msg_custom_button.button_index = 2;
- msg_custom_button.command_code = 0;
- this->studentCustomButtonPublisher.publish(msg_custom_button);
- ROS_INFO("Student button 2 pressed in GUI");
-}
-
-void MainWindow::on_studentButton_3_clicked()
-{
- CustomButton msg_custom_button;
- msg_custom_button.button_index = 3;
- 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;
- corrected_setpoint = setpoint;
-
- float x_size = context.localArea.xmax - context.localArea.xmin;
- float y_size = context.localArea.ymax - context.localArea.ymin;
- float z_size = context.localArea.zmax - context.localArea.zmin;
-
- if(setpoint.x > x_size/2)
- corrected_setpoint.x = x_size/2;
- if(setpoint.y > y_size/2)
- corrected_setpoint.y = y_size/2;
- if(setpoint.z > z_size)
- corrected_setpoint.z = z_size;
-
- if(setpoint.x < -x_size/2)
- corrected_setpoint.x = -x_size/2;
- if(setpoint.y < -y_size/2)
- corrected_setpoint.y = -y_size/2;
- if(setpoint.z < 0)
- corrected_setpoint.z = 0;
-
- return corrected_setpoint;
-}
-
-bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
-{
-
- float x_size = context.localArea.xmax - context.localArea.xmin;
- float y_size = context.localArea.ymax - context.localArea.ymin;
- float z_size = context.localArea.zmax - context.localArea.zmin;
- //position check
- if((setpoint.x < -x_size/2) or (setpoint.x > x_size/2)) {
- ROS_INFO_STREAM("x outside safety box");
- return false;
- }
- if((setpoint.y < -y_size/2) or (setpoint.y > y_size/2)) {
- ROS_INFO_STREAM("y outside safety box");
- return false;
- }
- if((setpoint.z < 0) or (setpoint.z > z_size)) {
- ROS_INFO_STREAM("z outside safety box");
- return false;
- }
-
- return true;
-}
-
-
-
-
-
-
-
-
-// # 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);
-}
-
-
-
-
-
-
-
-
-
-
-// PICKER CONTROLLER TAB
-
-// > FOR THE BUTTONS
-
-void MainWindow::send_picker_button_clicked_message(int button_index)
-{
- // Initialise the message
- std_msgs::Int32 msg;
- // Set the msg data
- msg.data = button_index;
- // Publish the message
- this->pickerButtonPressedPublisher.publish(msg);
-}
-
-void MainWindow::send_picker_button_clicked_message_with_setpoint(const SetpointV2& setpointV2_to_send)
-{
- // Publish the message
- this->pickerButtonPressedWithSetpointPublisher.publish(setpointV2_to_send);
-}
-
-void MainWindow::on_picker_gotostart_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_GOTOSTART);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_GOTOSTART;
- msg_setpointV2.isChecked = ui->picker_gotostart_checkBox->isChecked();
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Get the (x,y,z) coordinates from the GUI
- if(!ui->picker_gotostart_x->text().isEmpty())
- msg_setpointV2.x = (ui->picker_gotostart_x->text()).toFloat();
- if(!ui->picker_gotostart_y->text().isEmpty())
- msg_setpointV2.y = (ui->picker_gotostart_y->text()).toFloat();
- if(!ui->picker_gotostart_z->text().isEmpty())
- msg_setpointV2.z = (ui->picker_gotostart_z->text()).toFloat();
- // Update the z slider in the GUI
- ui->picker_z_slider->setValue( int((msg_setpointV2.z+0.005)*100.0) );
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_attach_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_ATTACH);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_ATTACH;
- msg_setpointV2.isChecked = ui->picker_attach_checkBox->isChecked();;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Get the z coordinates from the GUI
- if(!ui->picker_attach_z->text().isEmpty())
- msg_setpointV2.z = (ui->picker_attach_z->text()).toFloat();
- // Update the z slider in the GUI
- ui->picker_z_slider->setValue( int((msg_setpointV2.z+0.005)*100.0) );
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_pickup_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_PICKUP);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_PICKUP;
- msg_setpointV2.isChecked = ui->picker_pickup_checkBox->isChecked();;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Get the z coordinates from the GUI
- if(!ui->picker_pickup_z->text().isEmpty())
- msg_setpointV2.z = (ui->picker_pickup_z->text()).toFloat();
- // Update the z slider in the GUI
- ui->picker_z_slider->setValue( int((msg_setpointV2.z+0.005)*100.0) );
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_gotoend_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_GOTOEND);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_GOTOEND;
- msg_setpointV2.isChecked = ui->picker_gotoend_checkBox->isChecked();;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Get the (x,y) coordinates from the GUI
- if(!ui->picker_gotoend_x->text().isEmpty())
- msg_setpointV2.x = (ui->picker_gotoend_x->text()).toFloat();
- if(!ui->picker_gotoend_y->text().isEmpty())
- msg_setpointV2.y = (ui->picker_gotoend_y->text()).toFloat();
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_putdown_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_PUTDOWN);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_PUTDOWN;
- msg_setpointV2.isChecked = ui->picker_putdown_checkBox->isChecked();;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Get the z coordinates from the GUI
- if(!ui->picker_putdown_z->text().isEmpty())
- msg_setpointV2.z = (ui->picker_putdown_z->text()).toFloat();
- // Update the z slider in the GUI
- ui->picker_z_slider->setValue( int((msg_setpointV2.z+0.005)*100.0) );
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_squat_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_SQUAT);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_SQUAT;
- msg_setpointV2.isChecked = ui->picker_squat_checkBox->isChecked();;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Get the z coordinates from the GUI
- if(!ui->picker_squat_z->text().isEmpty())
- msg_setpointV2.z = (ui->picker_squat_z->text()).toFloat();
- // Update the z slider in the GUI
- ui->picker_z_slider->setValue( int((msg_setpointV2.z+0.005)*100.0) );
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_jump_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_JUMP);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_JUMP;
- msg_setpointV2.isChecked = ui->picker_jump_checkBox->isChecked();;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Get the z coordinates from the GUI
- if(!ui->picker_jump_z->text().isEmpty())
- msg_setpointV2.z = (ui->picker_jump_z->text()).toFloat();
- // Update the z slider in the GUI
- ui->picker_z_slider->setValue( int((msg_setpointV2.z+0.005)*100.0) );
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_1_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_1);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_1;
- msg_setpointV2.isChecked = true;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_2_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_2);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_2;
- msg_setpointV2.isChecked = true;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_3_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_3);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_3;
- msg_setpointV2.isChecked = true;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-void MainWindow::on_picker_4_button_clicked()
-{
- if (!shouldSendWithSetpoint_for_pickerButtons)
- {
- // Call the function that sends the message
- send_picker_button_clicked_message(PICKER_BUTTON_4);
- }
- else
- {
- // Construct the setpoint
- SetpointV2 msg_setpointV2;
- msg_setpointV2.buttonID = PICKER_BUTTON_4;
- msg_setpointV2.isChecked = true;
- msg_setpointV2.x = 0.0;
- msg_setpointV2.y = 0.0;
- msg_setpointV2.z = 0.0;
- msg_setpointV2.yaw = 0.0;
- // Call the function that sends the message
- send_picker_button_clicked_message_with_setpoint(msg_setpointV2);
- }
-}
-
-
-
-// > FOR THE SLIDERS AND DIAL
-
-void MainWindow::on_picker_x_slider_valueChanged(int value)
-{
- // Initialise the message
- std_msgs::Float32 msg;
- // Set the msg data
- msg.data = float(value) / 100.0f;
- // Publish the message
- this->pickerXAdjustmentPublisher.publish(msg);
-}
-void MainWindow::on_picker_y_slider_valueChanged(int value)
-{
- // Initialise the message
- std_msgs::Float32 msg;
- // Set the msg data
- msg.data = float(value) / 100.0f;
- // Publish the message
- this->pickerYAdjustmentPublisher.publish(msg);
-}
-void MainWindow::on_picker_z_slider_valueChanged(int value)
-{
- ROS_INFO_STREAM("[Student GUI] z slider int value " << value );
- // Initialise the message
- std_msgs::Float32 msg;
- // Set the msg data
- msg.data = float(value) / 100.0f;
- // Publish the message
- this->pickerZSetpointPublisher.publish(msg);
-}
-void MainWindow::on_picker_mass_slider_valueChanged(int value)
-{
- // Initialise the message
- std_msgs::Float32 msg;
- // Set the msg data
- msg.data = float(value);
- // Publish the message
- this->pickerMassPublisher.publish(msg);
-}
-void MainWindow::on_picker_yaw_dial_valueChanged(int value)
-{
- // Initialise the message
- std_msgs::Float32 msg;
- // Set the msg data
- msg.data = float(value) * DEG2RAD;
- // Publish the message
- this->pickerYawSetpointPublisher.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
deleted file mode 100644
index 7890bdbb412dc9f5fc7be68e8174594943510c65..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ /dev/null
@@ -1,5238 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>MainWindow</class>
- <widget class="QMainWindow" name="MainWindow">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>1500</width>
- <height>1000</height>
- </rect>
- </property>
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>1500</width>
- <height>1000</height>
- </size>
- </property>
- <property name="windowTitle">
- <string>MainWindow</string>
- </property>
- <widget class="QWidget" name="centralWidget">
- <layout class="QGridLayout" name="gridLayout">
- <item row="1" column="1">
- <layout class="QVBoxLayout" name="verticalLayout_4">
- <property name="leftMargin">
- <number>6</number>
- </property>
- <property name="topMargin">
- <number>6</number>
- </property>
- <property name="rightMargin">
- <number>6</number>
- </property>
- <property name="bottomMargin">
- <number>6</number>
- </property>
- <item>
- <widget class="QGroupBox" name="groupBox">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Minimum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="font">
- <font>
- <pointsize>18</pointsize>
- <weight>50</weight>
- <bold>false</bold>
- </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="QHBoxLayout" name="horizontalLayout">
- <property name="sizeConstraint">
- <enum>QLayout::SetMaximumSize</enum>
- </property>
- <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="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>70</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>750</width>
- <height>70</height>
- </size>
- </property>
- <property name="font">
- <font>
- <pointsize>16</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Disconnect</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="rf_status_label">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>95</width>
- <height>70</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>95</width>
- <height>70</height>
- </size>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="RF_Connect_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>70</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>750</width>
- <height>70</height>
- </size>
- </property>
- <property name="font">
- <font>
- <pointsize>16</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Connect</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLineEdit" name="voltage_field">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>100</width>
- <height>70</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>70</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="battery_status_label">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>50</width>
- <height>70</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>50</width>
- <height>70</height>
- </size>
- </property>
- <property name="font">
- <font>
- <pointsize>16</pointsize>
- <italic>true</italic>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="take_off_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>70</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>750</width>
- <height>70</height>
- </size>
- </property>
- <property name="font">
- <font>
- <pointsize>16</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Take Off</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="land_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>70</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>750</width>
- <height>70</height>
- </size>
- </property>
- <property name="font">
- <font>
- <pointsize>16</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Land</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="flying_state_label">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>90</width>
- <height>70</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>90</width>
- <height>70</height>
- </size>
- </property>
- <property name="font">
- <font>
- <pointsize>16</pointsize>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- <property name="alignment">
- <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="motors_OFF_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>70</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>750</width>
- <height>70</height>
- </size>
- </property>
- <property name="font">
- <font>
- <pointsize>16</pointsize>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Motors OFF</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <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>3</number>
- </property>
- <property name="usesScrollButtons">
- <bool>true</bool>
- </property>
- <property name="tabsClosable">
- <bool>false</bool>
- </property>
- <widget class="QWidget" name="tab_safe">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <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_picker">
- <attribute name="title">
- <string>Picker</string>
- </attribute>
- <layout class="QGridLayout" name="gridLayout_19">
- <item row="0" column="0">
- <layout class="QGridLayout" name="gridLayout_18">
- <property name="leftMargin">
- <number>6</number>
- </property>
- <property name="topMargin">
- <number>6</number>
- </property>
- <property name="rightMargin">
- <number>6</number>
- </property>
- <property name="bottomMargin">
- <number>6</number>
- </property>
- <item row="0" column="2">
- <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>
- <layout class="QVBoxLayout" name="verticalLayout_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="QLabel" name="label_47">
- <property name="text">
- <string>Yaw</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="label_54">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>+ve x-axis</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QDial" name="picker_yaw_dial">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimum">
- <number>-180</number>
- </property>
- <property name="maximum">
- <number>180</number>
- </property>
- <property name="singleStep">
- <number>5</number>
- </property>
- <property name="pageStep">
- <number>15</number>
- </property>
- <property name="value">
- <number>0</number>
- </property>
- <property name="sliderPosition">
- <number>0</number>
- </property>
- <property name="tracking">
- <bool>false</bool>
- </property>
- <property name="wrapping">
- <bool>true</bool>
- </property>
- <property name="notchTarget">
- <double>6.000000000000000</double>
- </property>
- <property name="notchesVisible">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="label_55">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>-ve x-axis</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item>
- <layout class="QVBoxLayout" name="verticalLayout_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>
- <widget class="QLabel" name="label_48">
- <property name="text">
- <string>z</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="label_51">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>1.0</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item>
- <layout class="QHBoxLayout" name="horizontalLayout_9">
- <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="QSlider" name="picker_z_slider">
- <property name="maximum">
- <number>100</number>
- </property>
- <property name="singleStep">
- <number>1</number>
- </property>
- <property name="pageStep">
- <number>2</number>
- </property>
- <property name="tracking">
- <bool>false</bool>
- </property>
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="invertedAppearance">
- <bool>false</bool>
- </property>
- <property name="invertedControls">
- <bool>false</bool>
- </property>
- <property name="tickPosition">
- <enum>QSlider::TicksBothSides</enum>
- </property>
- <property name="tickInterval">
- <number>10</number>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item>
- <widget class="QLabel" name="label_50">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>0.0</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item>
- <layout class="QVBoxLayout" name="verticalLayout_7">
- <property name="leftMargin">
- <number>6</number>
- </property>
- <property name="topMargin">
- <number>6</number>
- </property>
- <property name="rightMargin">
- <number>6</number>
- </property>
- <property name="bottomMargin">
- <number>6</number>
- </property>
- <item>
- <widget class="QLabel" name="label_49">
- <property name="text">
- <string>Mass</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="label_53">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>38</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item>
- <layout class="QHBoxLayout" name="horizontalLayout_11">
- <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="QSlider" name="picker_mass_slider">
- <property name="minimum">
- <number>28</number>
- </property>
- <property name="maximum">
- <number>38</number>
- </property>
- <property name="pageStep">
- <number>1</number>
- </property>
- <property name="tracking">
- <bool>false</bool>
- </property>
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="tickPosition">
- <enum>QSlider::TicksBothSides</enum>
- </property>
- <property name="tickInterval">
- <number>1</number>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item>
- <widget class="QLabel" name="label_52">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>28</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- </layout>
- </item>
- <item row="1" column="0">
- <layout class="QVBoxLayout" name="verticalLayout_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="QPushButton" name="picker_1_button">
- <property name="text">
- <string>Button 1</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="picker_2_button">
- <property name="text">
- <string>Button 2</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="picker_3_button">
- <property name="text">
- <string>Button 3</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="picker_4_button">
- <property name="text">
- <string>Button 4</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="1" column="2">
- <layout class="QVBoxLayout" name="verticalLayout_9">
- <item>
- <widget class="QLabel" name="label_56">
- <property name="text">
- <string>x-axis adjustment</string>
- </property>
- </widget>
- </item>
- <item>
- <layout class="QHBoxLayout" name="horizontalLayout_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>
- <widget class="QLabel" name="label_60">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>-0.1</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QSlider" name="picker_x_slider">
- <property name="minimum">
- <number>-10</number>
- </property>
- <property name="maximum">
- <number>10</number>
- </property>
- <property name="pageStep">
- <number>1</number>
- </property>
- <property name="tracking">
- <bool>false</bool>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="tickPosition">
- <enum>QSlider::TicksBothSides</enum>
- </property>
- <property name="tickInterval">
- <number>1</number>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="label_58">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>0.1</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item>
- <widget class="QLabel" name="label_57">
- <property name="text">
- <string>y-axis adjustment</string>
- </property>
- </widget>
- </item>
- <item>
- <layout class="QHBoxLayout" name="horizontalLayout_13">
- <property name="leftMargin">
- <number>6</number>
- </property>
- <property name="topMargin">
- <number>6</number>
- </property>
- <property name="rightMargin">
- <number>6</number>
- </property>
- <property name="bottomMargin">
- <number>6</number>
- </property>
- <item>
- <widget class="QLabel" name="label_61">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>-0.1</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QSlider" name="picker_y_slider">
- <property name="minimum">
- <number>-10</number>
- </property>
- <property name="maximum">
- <number>10</number>
- </property>
- <property name="pageStep">
- <number>1</number>
- </property>
- <property name="tracking">
- <bool>false</bool>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="tickPosition">
- <enum>QSlider::TicksBothSides</enum>
- </property>
- <property name="tickInterval">
- <number>1</number>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="label_59">
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>0.1</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- </layout>
- </item>
- <item row="0" column="0">
- <layout class="QGridLayout" name="gridLayout_20">
- <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="3" column="0">
- <widget class="QPushButton" name="picker_pickup_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Pick-up</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QCheckBox" name="picker_attach_checkBox">
- <property name="styleSheet">
- <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="6" column="1">
- <widget class="QCheckBox" name="picker_squat_checkBox">
- <property name="styleSheet">
- <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QCheckBox" name="picker_putdown_checkBox">
- <property name="styleSheet">
- <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QCheckBox" name="picker_gotoend_checkBox">
- <property name="styleSheet">
- <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QCheckBox" name="picker_pickup_checkBox">
- <property name="styleSheet">
- <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="7" column="1">
- <widget class="QCheckBox" name="picker_jump_checkBox">
- <property name="styleSheet">
- <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QLabel" name="label_63">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>y[m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QCheckBox" name="picker_gotostart_checkBox">
- <property name="styleSheet">
- <string notr="true">QCheckBox::indicator{ width:30px ; height:30px }</string>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_62">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>x[m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QPushButton" name="picker_gotostart_button">
- <property name="text">
- <string>Goto Start</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QPushButton" name="picker_gotoend_button">
- <property name="text">
- <string>Goto End</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QPushButton" name="picker_attach_button">
- <property name="text">
- <string>Attach</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="picker_gotostart_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_64">
- <property name="text">
- <string>Function:</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QPushButton" name="picker_squat_button">
- <property name="text">
- <string>Squat</string>
- </property>
- </widget>
- </item>
- <item row="7" column="0">
- <widget class="QPushButton" name="picker_jump_button">
- <property name="text">
- <string>Jump</string>
- </property>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QPushButton" name="picker_putdown_button">
- <property name="text">
- <string>Put down</string>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QLineEdit" name="picker_gotostart_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QLineEdit" name="picker_gotostart_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="2" column="4">
- <widget class="QLineEdit" name="picker_attach_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="0" column="4">
- <widget class="QLabel" name="label_65">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>z[m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <widget class="QLineEdit" name="picker_pickup_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="4" column="2">
- <widget class="QLineEdit" name="picker_gotoend_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="4" column="3">
- <widget class="QLineEdit" name="picker_gotoend_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="5" column="4">
- <widget class="QLineEdit" name="picker_putdown_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="6" column="4">
- <widget class="QLineEdit" name="picker_squat_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="7" column="4">
- <widget class="QLineEdit" name="picker_jump_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>100</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier 10 Pitch</family>
- </font>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_66">
- <property name="text">
- <string>Smooth</string>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- </layout>
- </item>
- <item row="1" column="0">
- <spacer name="verticalSpacer_7">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- </layout>
- </widget>
- <widget class="QWidget" name="tab_demo">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <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">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <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="0">
- <widget class="QLabel" name="label_72">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>roll [deg]</string>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_67">
- <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="7" column="0">
- <widget class="QLabel" name="label_68">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>y [m]</string>
- </property>
- </widget>
- </item>
- <item row="11" column="3">
- <widget class="QLabel" name="label_25">
- <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="5">
- <widget class="QLabel" name="label_27">
- <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="14" column="5">
- <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="font">
- <font>
- <pointsize>16</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Set setpoint</string>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <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>Current</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <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>Setpoint:</string>
- </property>
- </widget>
- </item>
- <item row="3" column="3">
- <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>x [m] =</string>
- </property>
- </widget>
- </item>
- <item row="9" column="3">
- <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>z [m] =</string>
- </property>
- </widget>
- </item>
- <item row="7" column="3">
- <widget class="QLabel" name="label_23">
- <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="5">
- <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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- </widget>
- </item>
- <item row="11" column="5">
- <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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <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="7" column="4">
- <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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="11" column="4">
- <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="9" column="4">
- <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="9" column="5">
- <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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- </widget>
- </item>
- <item row="7" column="5">
- <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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLabel" name="label_73">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Current</string>
- </property>
- </widget>
- </item>
- <item row="14" column="0">
- <widget class="QLabel" name="label_71">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>pitch [deg]</string>
- </property>
- </widget>
- </item>
- <item row="11" column="0">
- <widget class="QLabel" name="label_70">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>yaw [deg]</string>
- </property>
- </widget>
- </item>
- <item row="9" column="0">
- <widget class="QLabel" name="label_69">
- <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="1" column="2">
- <widget class="QLabel" name="label_74">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Difference</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="current_x_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="diff_x_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="7" column="1">
- <widget class="QLineEdit" name="current_y_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="7" column="2">
- <widget class="QLineEdit" name="diff_y_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="9" column="1">
- <widget class="QLineEdit" name="current_z_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="9" column="2">
- <widget class="QLineEdit" name="diff_z_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="11" column="1">
- <widget class="QLineEdit" name="current_yaw_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="11" column="2">
- <widget class="QLineEdit" name="diff_yaw_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="14" column="1">
- <widget class="QLineEdit" name="current_pitch_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="15" column="1">
- <widget class="QLineEdit" name="current_roll_student">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="16" column="2">
- <widget class="QPushButton" name="studentButton_1">
- <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 1</string>
- </property>
- </widget>
- </item>
- <item row="16" column="3">
- <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="text">
- <string>Commnd 2</string>
- </property>
- </widget>
- </item>
- <item row="16" column="4">
- <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="16" column="5">
- <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">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <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="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="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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Set setpoint</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLabel" name="label_31">
- <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_32">
- <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="3" column="0">
- <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>x [m] =</string>
- </property>
- </widget>
- </item>
- <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>yaw [deg] =</string>
- </property>
- </widget>
- </item>
- <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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- <property name="text">
- <string>z [m] =</string>
- </property>
- </widget>
- </item>
- <item row="7" column="0">
- <widget class="QLabel" name="label_36">
- <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="2">
- <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="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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="current_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>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <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="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="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="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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- </widget>
- </item>
- <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="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- </layout>
- </widget>
- <widget class="QWidget" name="tab_remote">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <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="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- </widget>
- </item>
- <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="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="QPushButton" name="remote_unsubscribe_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>UN-subscribe</string>
- </property>
- </widget>
- </item>
- <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="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Monospace</family>
- </font>
- </property>
- </widget>
- </item>
- <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>Subscribe</string>
- </property>
- </widget>
- </item>
- <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="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="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="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="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="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Monospace</family>
- </font>
- </property>
- </widget>
- </item>
- <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="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="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>Remote</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="2">
- <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="0">
- <widget class="QLabel" name="remote_roll_label">
- <property name="text">
- <string>Roll [deg]</string>
- </property>
- </widget>
- </item>
- <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>
- <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="maximumSize">
- <size>
- <width>16777215</width>
- <height>100</height>
- </size>
- </property>
- <property name="text">
- <string>DE-activate</string>
- </property>
- </widget>
- </item>
- <item>
- <spacer name="verticalSpacer_3">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- </layout>
- </item>
- </layout>
- </widget>
- <widget class="QWidget" name="tab">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <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_45">
- <property name="text">
- <string>Vertical Controller Gain (Vertikal Regler Verstaekung)</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="Line" name="line_13">
- <property name="lineWidth">
- <number>5</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="Line" name="line_15">
- <property name="lineWidth">
- <number>5</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="11" column="0">
- <spacer name="verticalSpacer_4">
- <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>10</height>
- </size>
- </property>
- </spacer>
- </item>
- <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>Test</string>
- </property>
- </widget>
- </item>
- <item row="6" column="1">
- <widget class="Line" name="line_16">
- <property name="lineWidth">
- <number>5</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <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="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <spacer name="verticalSpacer_5">
- <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="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="maximumSize">
- <size>
- <width>160</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Test</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_37">
- <property name="text">
- <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="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="maximumSize">
- <size>
- <width>160</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <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>
- </item>
- </layout>
- </widget>
- </widget>
- </item>
- <item>
- <widget class="Line" name="line">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item>
- <layout class="QGridLayout" name="gridLayout_17">
- <property name="leftMargin">
- <number>6</number>
- </property>
- <property name="topMargin">
- <number>6</number>
- </property>
- <property name="rightMargin">
- <number>6</number>
- </property>
- <property name="bottomMargin">
- <number>6</number>
- </property>
- <item row="8" column="1">
- <widget class="Line" name="line_20">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item row="9" column="2">
- <widget class="QPushButton" name="load_tuning_yaml_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="7" column="1">
- <widget class="Line" name="line_21">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item row="6" column="1">
- <widget class="Line" name="line_22">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item row="7" column="2">
- <widget class="QPushButton" name="load_mpc_yaml_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="8" column="2">
- <widget class="QPushButton" name="load_remote_yaml_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="6" column="2">
- <widget class="QPushButton" name="load_student_yaml_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="6" column="0">
- <widget class="QPushButton" name="enable_student_controller">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>30</height>
- </size>
- </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="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>30</height>
- </size>
- </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="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>30</height>
- </size>
- </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="5" column="0">
- <widget class="QPushButton" name="enable_demo_controller">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="10" 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="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="5" column="2">
- <widget class="QPushButton" name="load_demo_yaml_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="3" column="1">
- <widget class="Line" name="line_3">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLabel" name="label_22">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>30</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>30</height>
- </size>
- </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="5" column="1">
- <widget class="Line" name="line_4">
- <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="1" column="1">
- <widget class="Line" name="line_6">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item row="10" column="1">
- <widget class="Line" name="line_5">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item row="7" column="0">
- <widget class="QPushButton" name="enable_mpc_controller">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="8" column="0">
- <widget class="QPushButton" name="enable_remote_controller">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="9" column="1">
- <widget class="Line" name="line_19">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item row="9" column="0">
- <widget class="QPushButton" name="enable_tuning_controller">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</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="4" column="0">
- <widget class="QPushButton" name="enable_picker_controller">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Picker</string>
- </property>
- </widget>
- </item>
- <item row="4" column="2">
- <widget class="QPushButton" name="load_picker_yaml_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <pointsize>14</pointsize>
- </font>
- </property>
- <property name="text">
- <string>Picker</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>
- <widget class="QMenuBar" name="menuBar">
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>1500</width>
- <height>25</height>
- </rect>
- </property>
- </widget>
- <widget class="QToolBar" name="mainToolBar">
- <attribute name="toolBarArea">
- <enum>TopToolBarArea</enum>
- </attribute>
- <attribute name="toolBarBreak">
- <bool>false</bool>
- </attribute>
- </widget>
- <widget class="QStatusBar" name="statusBar"/>
- </widget>
- <layoutdefault spacing="6" margin="11"/>
- <resources/>
- <connections/>
-</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/rosNodeThread_for_studentGUI.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/rosNodeThread_for_studentGUI.cpp
deleted file mode 100644
index ce716b2d68d6591def161b4df756b544981c3002..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/rosNodeThread_for_studentGUI.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
-//
-// This file is part of D-FaLL-System.
-//
-// D-FaLL-System is free software: you can redistribute it and/or modify
-// it under the terms of the GNU General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// (at your option) any later version.
-//
-// D-FaLL-System is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
-//
-// You should have received a copy of the GNU General Public License
-// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
-//
-//
-// ----------------------------------------------------------------------------------
-// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
-// D D F aaa L L S Y Y S T E MM MM
-// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
-// D D F a aa L L S Y S T E M M
-// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
-//
-//
-// DESCRIPTION:
-// Takes care of creating a ros node thread.
-//
-// ----------------------------------------------------------------------------------
-
-
-#include "rosNodeThread_for_studentGUI.h"
-
-#include "d_fall_pps/CMRead.h"
-#include "d_fall_pps/CMUpdate.h"
-#include "d_fall_pps/CMCommand.h"
-
-
-rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name, QObject* parent)
- : QObject(parent),
- m_Init_argc(argc),
- m_pInit_argv(pArgv),
- m_node_name(node_name)
-
-{
- /** Constructor for the node thread **/
-}
-
-rosNodeThread::~rosNodeThread()
-{
- if (ros::isStarted())
- {
- ros::shutdown();
- ros::waitForShutdown();
- } // end if
- m_pThread->wait();
-} // end destructor
-
-bool rosNodeThread::init()
-{
- m_pThread = new QThread();
- this->moveToThread(m_pThread); // QObject method
-
- connect(m_pThread, SIGNAL(started()), this, SLOT(run()));
- ros::init(m_Init_argc, m_pInit_argv, m_node_name); // student_GUI is the name of this node
-
- if (!ros::master::check())
- {
- ROS_ERROR("Master not found, please check teacher computer is running and try again");
- return false; // do not start without ros.
- }
-
- ros::start();
- ros::Time::init();
- ros::NodeHandle nh("~");
-
- m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
-
- // clients for db services:
- m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
- m_update_db_client = nh.serviceClient<CMUpdate>("/CentralManagerService/Update", false);
- m_command_db_client = nh.serviceClient<CMCommand>("/CentralManagerService/Command", false);
-
- m_pThread->start();
- return true;
-} // set up the thread
-
-void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
-{
- emit newViconData(p_msg); //pass the message to other places
-}
-
-void rosNodeThread::run()
-{
- ros::Rate loop_rate(100);
- QMutex * pMutex;
- while (ros::ok())
- {
- pMutex = new QMutex();
-
- // geometry_msgs::Twist cmd_msg;
- pMutex->lock();
- // cmd_msg.linear.x = m_speed;
- // cmd_msg.angular.z = m_angle;
- pMutex->unlock();
- // ROS_INFO("RUNNING");
-
- // sim_velocity.publish(cmd_msg);
- ros::spinOnce();
- loop_rate.sleep();
- delete pMutex;
- } // do ros things.
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro
deleted file mode 100644
index 215f47e85bbff68fcc2f216447650b4078b98923..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro
+++ /dev/null
@@ -1,28 +0,0 @@
-#-------------------------------------------------
-#
-# Project created by QtCreator 2017-08-21T11:01:25
-#
-#-------------------------------------------------
-
-QT += core gui
-
-greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
-
-TARGET = studentGUI
-TEMPLATE = app
-
-INCLUDEPATH += $$PWD/include
-CONFIG += c++11
-
-SOURCES += \
- src/main.cpp \
- src/MainWindow.cpp
-
-HEADERS += \
- include/MainWindow.h \
-
-FORMS += \
- src/MainWindow.ui
-
-RESOURCES += \
- studentgui.qrc
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
deleted file mode 100644
index 8e1edbbfa62e3ca2e0c55ceaefa53f5f983a47e2..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user.1400dcd
+++ /dev/null
@@ -1,271 +0,0 @@
-<?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/GUI_Qt/studentGUI/studentGUI.pro.user.7257614 b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user.7257614
deleted file mode 100644
index 3b064d2339f42557e51b032dbc9c7d731163b999..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user.7257614
+++ /dev/null
@@ -1,336 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.2, 2017-09-05T11:57:14. -->
-<qtcreator>
- <data>
- <variable>EnvironmentId</variable>
- <value type="QByteArray">{72576140-2426-4e8d-b4f8-00ed8021ee7f}</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.ActiveTarget</variable>
- <value type="int">0</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.EditorSettings</variable>
- <valuemap type="QVariantMap">
- <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
- <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
- <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
- <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
- <value type="QString" key="language">Cpp</value>
- <valuemap type="QVariantMap" key="value">
- <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
- </valuemap>
- </valuemap>
- <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
- <value type="QString" key="language">QmlJS</value>
- <valuemap type="QVariantMap" key="value">
- <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
- </valuemap>
- </valuemap>
- <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
- <value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
- <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
- <value type="int" key="EditorConfiguration.IndentSize">4</value>
- <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
- <value type="int" key="EditorConfiguration.MarginColumn">80</value>
- <value type="bool" key="EditorConfiguration.MouseHiding">true</value>
- <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
- <value type="int" key="EditorConfiguration.PaddingMode">1</value>
- <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
- <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
- <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
- <value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
- <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
- <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
- <value type="int" key="EditorConfiguration.TabSize">8</value>
- <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
- <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
- <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
- <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
- <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
- <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
- </valuemap>
- </data>
- <data>
- <variable>ProjectExplorer.Project.PluginSettings</variable>
- <valuemap type="QVariantMap"/>
- </data>
- <data>
- <variable>ProjectExplorer.Project.Target.0</variable>
- <valuemap type="QVariantMap">
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop Qt 5.7.0 GCC 64bit</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop Qt 5.7.0 GCC 64bit</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">qt.57.gcc_64_kit</value>
- <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
- <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
- <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop_Qt_5_7_0_GCC_64bit-Debug</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
- <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
- <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
- <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Debug</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
- <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">2</value>
- <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop_Qt_5_7_0_GCC_64bit-Release</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
- <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
- <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
- <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
- <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
- <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.2">
- <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop_Qt_5_7_0_GCC_64bit-Profile</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
- <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">true</value>
- <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
- </valuemap>
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
- <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
- <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
- <value type="QString">-w</value>
- <value type="QString">-r</value>
- </valuelist>
- <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
- <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
- <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
- <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Profile</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
- <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
- <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">3</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
- <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
- <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
- <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
- <value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
- <value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
- <value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
- <value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
- <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
- <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
- <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
- <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
- <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
- <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
- <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
- <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
- <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
- <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
- <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
- <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
- <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
- <value type="int">0</value>
- <value type="int">1</value>
- <value type="int">2</value>
- <value type="int">3</value>
- <value type="int">4</value>
- <value type="int">5</value>
- <value type="int">6</value>
- <value type="int">7</value>
- <value type="int">8</value>
- <value type="int">9</value>
- <value type="int">10</value>
- <value type="int">11</value>
- <value type="int">12</value>
- <value type="int">13</value>
- <value type="int">14</value>
- </valuelist>
- <value type="int" key="PE.EnvironmentAspect.Base">2</value>
- <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">studentGUI</value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
- <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro</value>
- <value type="bool" key="QmakeProjectManager.QmakeRunConfiguration.UseLibrarySearchPath">true</value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">studentGUI.pro</value>
- <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
- <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory.default">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop_Qt_5_7_0_GCC_64bit-Debug</value>
- <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
- <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
- <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
- <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
- <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
- <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
- </valuemap>
- <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
- </valuemap>
- </data>
- <data>
- <variable>ProjectExplorer.Project.TargetCount</variable>
- <value type="int">1</value>
- </data>
- <data>
- <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
- <value type="int">18</value>
- </data>
- <data>
- <variable>Version</variable>
- <value type="int">18</value>
- </data>
-</qtcreator>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentgui.qrc b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentgui.qrc
deleted file mode 100644
index 0f6e7d8eebb915c0f0f16522394150bbc2d84ec1..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentgui.qrc
+++ /dev/null
@@ -1,19 +0,0 @@
-<RCC>
- <qresource prefix="/">
- <file>images/battery_20.png</file>
- <file>images/battery_40.png</file>
- <file>images/battery_60.png</file>
- <file>images/battery_80.png</file>
- <file>images/battery_empty.png</file>
- <file>images/battery_full.png</file>
- <file>images/rf_connected.png</file>
- <file>images/rf_connecting.png</file>
- <file>images/rf_disconnected.png</file>
- <file>images/battery_unknown.png</file>
- <file>images/flying_state_disabling.png</file>
- <file>images/flying_state_enabling.png</file>
- <file>images/flying_state_flying.png</file>
- <file>images/flying_state_off.png</file>
- <file>images/flying_state_unknown.png</file>
- </qresource>
-</RCC>
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
deleted file mode 100644
index aac50d36e36bb10588f730045fa5f06f85e85007..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
+++ /dev/null
@@ -1,321 +0,0 @@
-// 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
-#define PICKER_CONTROLLER 7
-
-// 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_USE_PICKER_CONTROLLER 7
-
-
-#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;
-// The Picker controller specified in the ClientConfig.yaml
-ros::ServiceClient pickerController;
-
-
-//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 loadPickerController();
-
-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/ParameterServiceDefinitions.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
deleted file mode 100644
index 9801abf3ae0cd73dbf65a6bcbe655dc4c0081aea..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
+++ /dev/null
@@ -1,76 +0,0 @@
-// 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_PICKER_CONTROLLER_AGENT 7
-
-#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
-#define LOAD_YAML_PICKER_CONTROLLER_COORDINATOR 17
-
-
-// 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_PICKER_CONTROLLER_FROM_OWN_AGENT 7
-
-#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
-#define FETCH_YAML_PICKER_CONTROLLER_FROM_COORDINATOR 17
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
deleted file mode 100644
index 60ad9e02f25b51f400e3549689202548cb40fc47..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
+++ /dev/null
@@ -1,467 +0,0 @@
-// 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/SetpointV2.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>
-#include <std_msgs/Float32.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
-
-#define RAD2DEG 180.0/PI
-#define DEG2RAD PI/180.0
-
-
-
-// FOR WHICH BUTTON WAS PRESSED IN THE PICKER CONTOLLER
-#define PICKER_BUTTON_GOTOSTART 1
-#define PICKER_BUTTON_ATTACH 2
-#define PICKER_BUTTON_PICKUP 3
-#define PICKER_BUTTON_GOTOEND 4
-#define PICKER_BUTTON_PUTDOWN 5
-#define PICKER_BUTTON_SQUAT 6
-#define PICKER_BUTTON_JUMP 7
-
-#define PICKER_BUTTON_1 11
-#define PICKER_BUTTON_2 12
-#define PICKER_BUTTON_3 13
-#define PICKER_BUTTON_4 14
-
-
-// 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 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
-// ----------------------------------------------------------------------------------
-
-// Current time
-int m_time_ticks = 0;
-float m_time_seconds;
-
-// > Mass of the Crazyflie quad-rotor, in [grams]
-float m_mass_CF_grams;
-
-// > Mass of the letters to be lifted, in [grams]
-float m_mass_E_grams;
-float m_mass_T_grams;
-float m_mass_H_grams;
-
-// > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
-float m_mass_total_grams;
-
-// Thickness of the object at pick-up and put-down, in [meters]
-// > This should also account for extra height due to
-// the surface where the object is
-float m_thickness_of_object_at_pickup;
-float m_thickness_of_object_at_putdown;
-
-// (x,y) coordinates of the pickup location
-std::vector<float> m_pickup_coordinates_xy(2);
-
-// (x,y) coordinates of the drop off location
-std::vector<float> m_dropoff_coordinates_xy_for_E(2);
-std::vector<float> m_dropoff_coordinates_xy_for_T(2);
-std::vector<float> m_dropoff_coordinates_xy_for_H(2);
-
-// Length of the string from the Crazyflie
-// to the end of the Picker, in [meters]
-float m_picker_string_length;
-
-// > The setpoints for (x,y,z) position and yaw angle, in that order
-float m_setpoint[4] = {0.0,0.0,0.4,0.0};
-float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
-
-// > Small adjustments to the x-y setpoint
-float m_xAdjustment = 0.0f;
-float m_yAdjustment = 0.0f;
-
-// Boolean for whether to limit rate of change of the setpoint
-bool m_shouldSmoothSetpointChanges = true;
-
-// Max setpoint change per second
-float m_max_setpoint_change_per_second_horizontal;
-float m_max_setpoint_change_per_second_vertical;
-float m_max_setpoint_change_per_second_yaw_degrees;
-float m_max_setpoint_change_per_second_yaw_radians;
-
-
-// Frequency at which the controller is running
-float m_vicon_frequency;
-
-
-
-
-
-// THE FOLLOWING PARAMETERS ARE USED
-// FOR THE LOW-LEVEL CONTROLLER
-
-// Frequency at which the controller is running
-float control_frequency;
-
-// > Coefficients of the 16-bit command to thrust conversion
-std::vector<float> motorPoly(3);
-
-// 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 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;
-
-
-// ROS Publisher for my current (x,y,z,yaw) position
-ros::Publisher my_current_xyz_yaw_publisher;
-
-// ROS Publisher for the current setpoint
-ros::Publisher pickerSetpointToGUIPublisher;
-
-
-// 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.
-
-
-// ADDED FOR THE PICKER
-void perControlCycleOperations();
-
-// CALLBACK FROM ROS MESSAGES RECEIVED
-void buttonPressedCallback(const std_msgs::Int32& msg);
-void zSetpointCallback(const std_msgs::Float32& msg);
-void yawSetpointCallback(const std_msgs::Float32& msg);
-void massCallback(const std_msgs::Float32& msg);
-void xAdjustmentCallback(const std_msgs::Float32& msg);
-void yAdjustmentCallback(const std_msgs::Float32& msg);
-
-void buttonPressedWithSetpointCallback(const SetpointV2& newSetpointV2);
-
-
-// SEPARATE CALLBACK FUNCTIONS FOR EACH BUTTON
-void buttonPressed_gotoStart();
-void buttonPressed_attach();
-void buttonPressed_pickup();
-void buttonPressed_gotoEnd();
-void buttonPressed_putdown();
-void buttonPressed_squat();
-void buttonPressed_jump();
-
-void buttonPressed_1();
-void buttonPressed_2();
-void buttonPressed_3();
-void buttonPressed_4();
-
-
-// SEPARATE CALLBACK FUNCTIONS FOR EACH BUTTON
-// > WITH A SETPOINT IN THE MESSAGE
-void buttonPressedWithSetpoint_gotoStart(const SetpointV2& newSetpointV2);
-void buttonPressedWithSetpoint_attach(const SetpointV2& newSetpointV2);
-void buttonPressedWithSetpoint_pickup(const SetpointV2& newSetpointV2);
-void buttonPressedWithSetpoint_gotoEnd(const SetpointV2& newSetpointV2);
-void buttonPressedWithSetpoint_putdown(const SetpointV2& newSetpointV2);
-void buttonPressedWithSetpoint_squat(const SetpointV2& newSetpointV2);
-void buttonPressedWithSetpoint_jump(const SetpointV2& newSetpointV2);
-
-void buttonPressedWithSetpoint_1(const SetpointV2& newSetpointV2);
-void buttonPressedWithSetpoint_2(const SetpointV2& newSetpointV2);
-void buttonPressedWithSetpoint_3(const SetpointV2& newSetpointV2);
-void buttonPressedWithSetpoint_4(const SetpointV2& newSetpointV2);
-
-
-
-
-
-
-
-// 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);
-
-void publishCurrentSetpoint();
-
-// 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/StudentControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
deleted file mode 100644
index 818993f0758f58f4a980ab1352944764831bf6a0..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
+++ /dev/null
@@ -1,224 +0,0 @@
-// 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/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_in_grams = 25.0; // 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 cf_weight_in_newtons = 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/launch/Config.sh b/pps_ws/src/d_fall_pps/launch/Config.sh
deleted file mode 100755
index 48d929ff5876d90a5548b4e1d0a49fcf657b3ac1..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/launch/Config.sh
+++ /dev/null
@@ -1,6 +0,0 @@
-export ROS_MASTER_URI=http://teacher:11311
-export ROS_IP=$(hostname -I | awk '{print $1;}')
-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
deleted file mode 100755
index c77f5104509c44d70d789a74826306bccfd05976..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/launch/Coordinator.launch
+++ /dev/null
@@ -1,27 +0,0 @@
-<launch>
-
- <!-- INPUT ARGUMENT OF THE AGENT's ID -->
- <arg name="coordID" default="$(optenv DFALL_DEFAULT_COORD_ID)" />
-
- <!-- Example of how to use the value in coordID -->
- <!-- <param name="param" value="$(arg coordID)"/> -->
-
- <!-- Example of how to specify the coordID from command line -->
- <!-- roslaunch d_fall_pps coordID:=001 -->
-
- <group ns="coord$(arg coordID)">
-
- <!-- COORDINATOR GUI -->
- <node
- pkg="d_fall_pps"
- name="flyingAgentGUI"
- output="screen"
- type="flyingAgentGUI"
- >
- <param name="type" type="str" value="coordinator" />
- <param name="coordID" type="str" value="$(arg coordID)" />
- </node>
-
- </group>
-
-</launch>
diff --git a/pps_ws/src/d_fall_pps/launch/Teacher.launch b/pps_ws/src/d_fall_pps/launch/Teacher.launch
deleted file mode 100755
index ef7b20a73aaf513096282fc723b2e33c8fe9d3d7..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/launch/Teacher.launch
+++ /dev/null
@@ -1,21 +0,0 @@
-<launch>
- <node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
- </node>
-
- <node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
- <rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
- </node>
-
- <!-- 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">
- </node>
-
- <!-- PARAMETER SERVICE -->
- <node pkg="d_fall_pps" name="ParameterService" output="screen" type="ParameterService">
- <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/DemoController.yaml" ns="CustomController" />
- </node>
-
-</launch>
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
deleted file mode 100755
index 8b15a220a9b8c73b84a1b27bdf84a53946a7705c..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ /dev/null
@@ -1,15 +0,0 @@
-safeController: "SafeControllerService/RateController"
-demoController: "DemoControllerService/DemoController"
-studentController: "StudentControllerService/StudentController"
-mpcController: "MpcControllerService/MpcController"
-remoteController: "RemoteControllerService/RemoteController"
-tuningController: "TuningControllerService/TuningController"
-pickerController: "PickerControllerService/PickerController"
-
-strictSafety: false
-angleMargin: 0.8
-
-battery_threshold_while_flying: 2.60 # in V
-battery_threshold_while_motors_off: 3.30 # in V
-battery_polling_period: 200 # in ms
-
diff --git a/pps_ws/src/d_fall_pps/param/Crazyflie.db b/pps_ws/src/d_fall_pps/param/Crazyflie.db
deleted file mode 100644
index a9616e9d9abc2f565f79103502a8f815dcae32b3..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/param/Crazyflie.db
+++ /dev/null
@@ -1,8 +0,0 @@
-1,CF01,0/0/2M/E7E7E7E701,0,-1.95374,-0.0143279,-0.2,-1.01034,0.774692,1.2
-2,CF02,0/8/2M/E7E7E7E702,1,-0.844536,-0.025763,-0.2,0.0759869,0.751821,1.2
-3,CF03,0/16/2M/E7E7E7E703,2,0.213208,0.00282468,-0.2,1.18519,0.757539,1.2
-4,CF04,0/24/2M/E7E7E7E704,3,1.34528,-0.0143279,-0.2,2.31154,0.723234,1.2
-5,CF05,0/32/2M/E7E7E7E705,4,1.351,0.791844,-0.2,2.31154,1.54084,1.2
-6,CF06,0/40/2M/E7E7E7E706,5,0.20749,0.843302,-0.2,1.18519,1.55799,1.2
-7,CF07,0/48/2M/E7E7E7E707,6,-0.867406,0.843302,-0.2,0.0702693,1.54656,1.2
-8,CF08,0/56/2M/E7E7E7E708,7,-1.95374,0.854737,-0.2,-0.987474,1.54656,1.2
diff --git a/pps_ws/src/d_fall_pps/param/StudentController.yaml b/pps_ws/src/d_fall_pps/param/StudentController.yaml
deleted file mode 100644
index 7dbb3ec7780a586be493fa688907a364ad214061..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/param/StudentController.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-# Mass of the crazyflie
-mass : 28
-
-# 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]
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/param/YamlFileNames.yaml b/pps_ws/src/d_fall_pps/param/YamlFileNames.yaml
deleted file mode 100644
index 53fd75c2ba6f14e86dfcd8e8809314c35fd9a8af..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/param/YamlFileNames.yaml
+++ /dev/null
@@ -1,4 +0,0 @@
-dictionary : {
- 'ClientConfig' : 'ClientConfig' ,
- 'SafeController' : 'SafeController'
-}
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
deleted file mode 100755
index 144ddd19c4944b7534c2ca47ed2bc105e512ce64..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ /dev/null
@@ -1,1133 +0,0 @@
-// 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.
-//
-// ----------------------------------------------------------------------------------
-
-
-
-
-
-// INCLUDE THE HEADER
-#include "nodes/PPSClient.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
-// ----------------------------------------------------------------------------------
-
-
-
-
-
-
-
-
-//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("[PPS CLIENT] x safety failed");
- return false;
- }
- if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
- 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("[PPS CLIENT] z safety failed");
- return false;
- }
-
- //attitude check
- //if strictSafety is set to true in ClientConfig.yaml the SafeController takes also over if the roll and pitch angles get to large
- //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("[PPS CLIENT] roll too big.");
- return false;
- }
- if((data.pitch > PI/2*angleMargin) or (data.pitch < -PI/2*angleMargin)) {
- ROS_INFO_STREAM("[PPS CLIENT] pitch too big.");
- return false;
- }
- }
-
- return true;
-}
-
-void coordinatesToLocal(CrazyflieData& cf) {
- AreaBounds area = context.localArea;
- float originX = (area.xmin + area.xmax) / 2.0;
- float originY = (area.ymin + area.ymax) / 2.0;
- // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
- float originZ = 0.0;
- // float originZ = (area.zmin + area.zmax) / 2.0;
-
- cf.x -= originX;
- cf.y -= originY;
- cf.z -= originZ;
-}
-
-
-void setCurrentSafeSetpoint(Setpoint setpoint)
-{
- current_safe_setpoint = setpoint;
-}
-
-void calculateDistanceToCurrentSafeSetpoint(CrazyflieData& local)
-{
- double dx = current_safe_setpoint.x - local.x;
- double dy = current_safe_setpoint.y - local.y;
- double dz = current_safe_setpoint.z - local.z;
-
- distance = sqrt(pow(dx,2) + pow(dy,2) + pow(dz,2));
-
- unit_vector[0] = dx/distance;
- unit_vector[1] = dy/distance;
- unit_vector[2] = dz/distance;
-}
-
-
-void takeOffCF(CrazyflieData& current_local_coordinates) //local because the setpoint is in local coordinates
-{
- // set the setpoint and call safe controller
- Setpoint setpoint_msg;
- setpoint_msg.x = current_local_coordinates.x; // previous one
- setpoint_msg.y = current_local_coordinates.y; //previous one
- setpoint_msg.z = current_local_coordinates.z + take_off_distance; //previous one plus some offset
- // setpoint_msg.yaw = current_local_coordinates.yaw; //previous one
- setpoint_msg.yaw = 0.0;
- safeControllerServiceSetpointPublisher.publish(setpoint_msg);
- 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();
- setInstantController(SAFE_CONTROLLER);
- // when do we finish? after some time with delay?
-
- // update variable that keeps track of current setpoint
- setCurrentSafeSetpoint(setpoint_msg);
-}
-
-void landCF(CrazyflieData& current_local_coordinates)
-{
- // set the setpoint and call safe controller
- Setpoint setpoint_msg;
- setpoint_msg.x = current_local_coordinates.x; // previous one
- setpoint_msg.y = current_local_coordinates.y; //previous one
- setpoint_msg.z = landing_distance; //previous one plus some offset
- setpoint_msg.yaw = current_local_coordinates.yaw; //previous one
- safeControllerServiceSetpointPublisher.publish(setpoint_msg);
-
- // now, use safe controller to go to that setpoint
- loadSafeController();
- setInstantController(SAFE_CONTROLLER);
- setCurrentSafeSetpoint(setpoint_msg);
-}
-
-void changeFlyingStateTo(int new_state)
-{
- if(crazyradio_status == CONNECTED)
- {
- ROS_INFO("[PPS CLIENT] Change state to: %d", new_state);
- flying_state = new_state;
- }
- else
- {
- ROS_INFO("[PPS CLIENT] Disconnected and trying to change state. State goes to MOTORS OFF");
- flying_state = STATE_MOTORS_OFF;
- }
-
- changed_state_flag = true;
- std_msgs::Int32 flying_state_msg;
- flying_state_msg.data = flying_state;
- flyingStatePublisher.publish(flying_state_msg);
-}
-
-
-void takeOffTimerCallback(const ros::TimerEvent&)
-{
- finished_take_off = true;
-}
-
-void landTimerCallback(const ros::TimerEvent&)
-{
- finished_land = true;
-}
-
-void goToControllerSetpoint()
-{
- safeControllerServiceSetpointPublisher.publish(controller_setpoint);
- setCurrentSafeSetpoint(controller_setpoint);
-}
-
-
-//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 global = *it;
-
- if(global.crazyflieName == context.crazyflieName)
- {
- Controller controllerCall;
- CrazyflieData local = global;
- coordinatesToLocal(local);
- controllerCall.request.ownCrazyflie = local;
-
- ros::NodeHandle nodeHandle("~");
-
- switch(flying_state) //things here repeat every X ms, non-blocking stuff
- {
- case STATE_MOTORS_OFF: // here we will put the code of current disabled button
- if(changed_state_flag) // stuff that will be run only once when changing state
- {
- changed_state_flag = false;
- ROS_INFO("[PPS CLIENT] STATE_MOTORS_OFF");
- }
- break;
- case STATE_TAKE_OFF: //we need to move up from where we are now.
- if(changed_state_flag) // stuff that will be run only once when changing state
- {
- changed_state_flag = false;
- takeOffCF(local);
- finished_take_off = false;
- ROS_INFO("[PPS CLIENT] STATE_TAKE_OFF");
- timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_take_off), takeOffTimerCallback, true);
- }
- if(finished_take_off)
- {
- finished_take_off = false;
- setInstantController(getControllerUsed());
- changeFlyingStateTo(STATE_FLYING);
- }
- break;
- case STATE_FLYING:
- if(changed_state_flag) // stuff that will be run only once when changing state
- {
- changed_state_flag = false;
- // need to change setpoint to the controller one
- goToControllerSetpoint();
- ROS_INFO("[PPS CLIENT] STATE_FLYING");
- }
- break;
- case STATE_LAND:
- if(changed_state_flag) // stuff that will be run only once when changing state
- {
- changed_state_flag = false;
- landCF(local);
- finished_land = false;
- ROS_INFO("[PPS CLIENT] STATE_LAND");
- timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_landing), landTimerCallback, true);
- }
- if(finished_land)
- {
- finished_land = false;
- setInstantController(getControllerUsed());
- changeFlyingStateTo(STATE_MOTORS_OFF);
- }
- break;
- default:
- break;
- }
-
- // control part that should always be running, calls to controller, computation of control output
- if(flying_state != STATE_MOTORS_OFF)
- {
- if(!global.occluded) //if it is not occluded, then proceed to compute the controller output.
- {
- // 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) )
- {
- // 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;
- case PICKER_CONTROLLER:
- success = pickerController.call(controllerCall);
- break;
- default:
- ROS_ERROR("[PPS CLIENT] the current controller was not recognised");
- break;
- }
-
- // Ensure success and enforce safety
- if(!success)
- {
- 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("[PPS CLIENT] safety check failed, switching to safe controller");
- }
-
-
- }
- // SAFE_CONTROLLER and state is different from flying
- else
- {
- calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
- // ROS_INFO_STREAM("distance: " << distance);
- // here, detect if euclidean distance between setpoint and current position is higher than a threshold
- if(distance > distance_threshold)
- {
- // DEBUGGING: display a message that the crazyflie is inside the thresholds
- //ROS_INFO("inside threshold");
- // Declare a local variable for the "setpoint message" to be published
- Setpoint setpoint_msg;
- // here, where we are now, or where we were in the beginning?
- setpoint_msg.x = local.x + distance_threshold * unit_vector[0];
- setpoint_msg.y = local.y + distance_threshold * unit_vector[1];
- setpoint_msg.z = local.z + distance_threshold * unit_vector[2];
-
- // yaw is better divided by the number of steps?
- setpoint_msg.yaw = current_safe_setpoint.yaw;
- safeControllerServiceSetpointPublisher.publish(setpoint_msg);
- was_in_threshold = true;
- }
- else
- {
- if(was_in_threshold)
- {
- was_in_threshold = false;
- safeControllerServiceSetpointPublisher.publish(current_safe_setpoint);
- // goToControllerSetpoint(); //maybe this is a bit repetitive?
- }
- }
-
- bool success = safeController.call(controllerCall);
- if(!success) {
- ROS_ERROR_STREAM("[PPS CLIENT] Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
- }
- }
-
-
- controlCommandPublisher.publish(controllerCall.response.controlOutput);
-
- // Putting data into the ROS "bag" for post-analysis
- //bag.write("ViconData", ros::Time::now(), local);
- //bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
- }
- }
- else
- {
- ControlCommand zeroOutput = ControlCommand(); //everything set to zero
- zeroOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS; //set to motor_mode
- controlCommandPublisher.publish(zeroOutput);
-
- // Putting data into the ROS "bag" for post-analysis
- //bag.write("ViconData", ros::Time::now(), local);
- //bag.write("ControlOutput", ros::Time::now(), zeroOutput);
- }
- }
- }
-}
-
-
-
-
-void loadCrazyflieContext() {
- CMQuery contextCall;
- contextCall.request.studentID = agentID;
- ROS_INFO_STREAM("[PPS CLIENT] AgentID:" << agentID);
-
- CrazyflieContext new_context;
-
- centralManager.waitForExistence(ros::Duration(-1));
-
- if(centralManager.call(contextCall)) {
- new_context = contextCall.response.crazyflieContext;
- 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
- {
-
- // Motors off is done in python script now everytime we disconnect
-
- // send motors OFF and disconnect before setting context = new_context
- // std_msgs::Int32 msg;
- // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
- // commandPublisher.publish(msg);
-
- ROS_INFO("[PPS CLIENT] CF is now different for this student. Disconnect and turn it off");
-
- std_msgs::Int32 msg;
- msg.data = CMD_DISCONNECT;
- crazyRadioCommandPublisher.publish(msg);
- }
-
- context = new_context;
-
- ros::NodeHandle nh("CrazyRadio");
- nh.setParam("crazyFlieAddress", context.crazyflieAddress);
- }
- else
- {
- ROS_ERROR("[PPS CLIENT] Failed to load context. Waiting for next Save in DB by teacher");
- }
-}
-
-
-
-void commandCallback(const std_msgs::Int32& commandMsg) {
- int cmd = commandMsg.data;
-
- switch(cmd) {
- case CMD_USE_SAFE_CONTROLLER:
- ROS_INFO("[PPS CLIENT] USE_SAFE_CONTROLLER Command received");
- setControllerUsed(SAFE_CONTROLLER);
- break;
-
- 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_USE_PICKER_CONTROLLER:
- ROS_INFO("[PPS CLIENT] USE_PICKER_CONTROLLER Command received");
- setControllerUsed(PICKER_CONTROLLER);
- break;
-
- case CMD_CRAZYFLY_TAKE_OFF:
- if(flying_state == STATE_MOTORS_OFF)
- {
- changeFlyingStateTo(STATE_TAKE_OFF);
- }
- break;
-
- case CMD_CRAZYFLY_LAND:
- if(flying_state != STATE_MOTORS_OFF)
- {
- changeFlyingStateTo(STATE_LAND);
- }
- break;
- case CMD_CRAZYFLY_MOTORS_OFF:
- changeFlyingStateTo(STATE_MOTORS_OFF);
- break;
-
- default:
- ROS_ERROR_STREAM("[PPS CLIENT] unexpected command number: " << cmd);
- break;
- }
-}
-
-void DBChangedCallback(const std_msgs::Int32& msg)
-{
- loadCrazyflieContext();
-}
-
-void emergencyStopCallback(const std_msgs::Int32& msg)
-{
- commandCallback(msg);
-}
-
-void commandAllAgentsCallback(const std_msgs::Int32& msg)
-{
- commandCallback(msg);
-}
-
-void crazyRadioStatusCallback(const std_msgs::Int32& msg)
-{
- crazyradio_status = msg.data;
- // RESET THE BATTERY STATE IF DISCONNECTED
- if (crazyradio_status == DISCONNECTED)
- {
- setBatteryStateTo(BATTERY_STATE_NORMAL);
- }
-}
-
-void controllerSetPointCallback(const Setpoint& newSetpoint)
-{
- // load in variable the setpoint
- controller_setpoint = newSetpoint;
-
- // if we are in flying, set it up NOW
- if(flying_state == STATE_FLYING)
- {
- goToControllerSetpoint();
- }
-}
-
-void safeSetPointCallback(const Setpoint& newSetpoint)
-{
-}
-
-
-
-
-// ----------------------------------------------------------------------------------
-// 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
-// ----------------------------------------------------------------------------------
-
-
-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_SAFE_CONTROLLER_FROM_OWN_AGENT:
- {
- // Let the user know that this message was received
- 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
- fetchYamlParametersForSafeController(nodeHandle_to_own_agent_parameter_service);
- break;
- }
-
- // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- 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("[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
- fetchYamlParametersForSafeController(nodeHandle_to_coordinator_parameter_service);
- break;
- }
-
- default:
- {
- // Let the user know that the command was not relevant
- //ROS_INFO("The PPSClient received the message that YAML parameters were (re-)loaded.\r> However the parameters do not relate to this service, hence nothing will be fetched.");
- break;
- }
- }
-}
-
-
-
-void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle)
-{
- // Here we load the parameters that are specified in the SafeController.yaml file
-
- // 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("[PPS CLIENT] Failed to get takeOffDistance");
- }
-
- if(!nodeHandle_for_safeController.getParam("landingDistance", landing_distance))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get landing_distance");
- }
-
- if(!nodeHandle_for_safeController.getParam("durationTakeOff", duration_take_off))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get duration_take_off");
- }
-
- if(!nodeHandle_for_safeController.getParam("durationLanding", duration_landing))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get duration_landing");
- }
-
- if(!nodeHandle_for_safeController.getParam("distanceThreshold", distance_threshold))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get distance_threshold");
- }
-}
-
-
-// > Load the paramters from the Client Config YAML file
-void fetchClientConfigParameters(ros::NodeHandle& nodeHandle)
-{
- if(!nodeHandle.getParam("agentID", agentID)) {
- ROS_ERROR("[PPS CLIENT] Failed to get agentID");
- }
- if(!nodeHandle.getParam("strictSafety", strictSafety)) {
- ROS_ERROR("[PPS CLIENT] Failed to get strictSafety param");
- return;
- }
- if(!nodeHandle.getParam("angleMargin", angleMargin)) {
- ROS_ERROR("[PPS CLIENT] Failed to get angleMargin param");
- return;
- }
- if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
- 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("[PPS CLIENT] Failed to get battery_threshold_while_motors_off param");
- return;
- }
-}
-
-
-
-
-
-
-
-
-
-
-void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg)
-{
- // The "msg" received can be directly published on the "crazyRadioCommandPublisher"
- // class variable because it is same format message
- // > NOTE: this may be inefficient to "just" pass on the message,
- // the intention is that it is more transparent that the "coordinator"
- // node requests all agents to (re/dis)-connect from, and the
- // individual agents pass this along to their respective radio node.
- crazyRadioCommandPublisher.publish(msg);
-}
-
-
-
-
-
-
-
-
-
-int getBatteryState()
-{
- return m_battery_state;
-}
-
-
-void setBatteryStateTo(int new_battery_state)
-{
- switch(new_battery_state)
- {
- case BATTERY_STATE_NORMAL:
- m_battery_state = BATTERY_STATE_NORMAL;
- ROS_INFO("[PPS CLIENT] changed battery state to normal");
- break;
- case BATTERY_STATE_LOW:
- m_battery_state = BATTERY_STATE_LOW;
- ROS_INFO("[PPS CLIENT] changed battery state to low");
- if(flying_state != STATE_MOTORS_OFF)
- changeFlyingStateTo(STATE_LAND);
- break;
- default:
- ROS_INFO("[PPS CLIENT] Unknown battery state command, set to normal");
- m_battery_state = BATTERY_STATE_NORMAL;
- break;
- }
-
- std_msgs::Int32 battery_state_msg;
- battery_state_msg.data = getBatteryState();
- batteryStatePublisher.publish(battery_state_msg);
-}
-
-float movingAverageBatteryFilter(float new_input)
-{
- const int N = 7;
- static float previous_output = 4.2f;
- static float inputs[N] = {4.2f,4.2f,4.2f,4.2f,4.2f,4.2f,4.2f};
-
-
- // imagine an array of an even number of samples, we will output the one
- // in the middle averaged with information from all of them.
- // For that, we only need to store some past of the signal
- float output = previous_output + new_input/N - inputs[N-1]/N;
-
- // update array of inputs
- for(int i = N - 1; i > 0; i--)
- {
- inputs[i] = inputs[i-1];
- }
- inputs[0] = new_input;
-
-
- // update previous output
- previous_output = output;
- return output;
-}
-
-
-void CFBatteryCallback(const std_msgs::Float32& msg)
-{
- m_battery_voltage = msg.data;
- // filter and check if inside limits, and if, change status
- // need to do the filtering first
- float filtered_battery_voltage = movingAverageBatteryFilter(m_battery_voltage); //need to perform filtering here
-
- // ROS_INFO_STREAM("filtered data: " << filtered_battery_voltage);
- if(
- (flying_state != STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_flying))
- ||
- (flying_state == STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_motors_off))
- )
- {
- if(getBatteryState() != BATTERY_STATE_LOW)
- {
- setBatteryStateTo(BATTERY_STATE_LOW);
- ROS_INFO("[PPS CLIENT] low level battery triggered");
- }
-
- }
- else
- {
- // TO AVOID BEING ABLE TO IMMEDIATELY TAKE-OFF AFTER A
- // "BATTERY LOW" EVENT IS TRIGGERED, WE DO NOT SET THE
- // BATTERY STATE BACK TO NORMAL
- // if(getBatteryState() != BATTERY_STATE_NORMAL)
- // {
- // setBatteryStateTo(BATTERY_STATE_NORMAL);
- // }
- }
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-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 loadPickerController()
-{
- ros::NodeHandle nodeHandle("~");
-
- std::string pickerControllerName;
- if(!nodeHandle.getParam("pickerController", pickerControllerName))
- {
- ROS_ERROR("[PPS CLIENT] Failed to get picker controller name");
- return;
- }
-
- pickerController = ros::service::createClient<Controller>(pickerControllerName, true);
- ROS_INFO_STREAM("[PPS CLIENT] Loaded picker controller " << pickerController.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();
- case PICKER_CONTROLLER:
- loadPickerController();
- 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
-// M M M A A I N N N
-// M M AAAAA I N NN
-// M M A A III N N
-// ----------------------------------------------------------------------------------
-
-int main(int argc, char* argv[])
-{
- ros::init(argc, argv, "PPSClient");
- ros::NodeHandle nodeHandle("~");
- ros_namespace = ros::this_node::getNamespace();
-
- // *********************************************************************************
- // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
-
- // > Load the paramters from the Client Config YAML file
- fetchClientConfigParameters(nodeHandle);
-
- // Get the namespace of this "SafeControllerService" node
- std::string m_namespace = ros::this_node::getNamespace();
-
-
- // 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 = "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("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
-
-
- // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
-
- // Call the class function that loads the parameters for this class.
- fetchYamlParametersForSafeController(nodeHandle_to_own_agent_parameter_service);
-
- // *********************************************************************************
-
-
- // Load default setpoint from the "SafeController" namespace of the "ParameterService"
- std::vector<float> default_setpoint(4);
- ros::NodeHandle nodeHandle_for_safeController(nodeHandle_to_own_agent_parameter_service, "SafeController");
-
- if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
- {
- ROS_ERROR_STREAM("[PPS CLIENT] Could not find parameter 'defaultSetpoint', as called from main(...)");
- }
-
- // Copy the default setpoint into the class variable
- controller_setpoint.x = default_setpoint[0];
- controller_setpoint.y = default_setpoint[1];
- controller_setpoint.z = default_setpoint[2];
- controller_setpoint.yaw = default_setpoint[3];
-
- //ros::service::waitForService("/CentralManagerService/CentralManager");
- centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
- loadCrazyflieContext();
-
- //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
- ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
- ROS_INFO_STREAM("[PPS CLIENT] successfully subscribed to ViconData");
-
- //ros::Publishers to advertise the control output
- controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
-
- //this topic lets the PPSClient listen to the terminal commands
- commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
- ros::Subscriber commandSubscriber = nodeHandle.subscribe("Command", 1, commandCallback);
-
- //this topic lets us use the terminal to communicate with crazyRadio node.
- crazyRadioCommandPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
-
- // this topic will publish flying state whenever it changes.
- flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
-
- // will publish battery state when it changes
- batteryStatePublisher = nodeHandle.advertise<std_msgs::Int32>("batteryState", 1);
-
- controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerUsed", 1);
-
- // crazy radio status
- crazyradio_status = DISCONNECTED;
-
- // publish first flying state data
- std_msgs::Int32 flying_state_msg;
- flying_state_msg.data = flying_state;
- flyingStatePublisher.publish(flying_state_msg);
-
- // SafeControllerServicePublisher:
- ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
- safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1);
- ros::Subscriber controllerSetpointSubscriber = namespaceNodeHandle.subscribe("student_GUI/ControllerSetpoint", 1, controllerSetPointCallback);
- ros::Subscriber safeSetpointSubscriber = namespaceNodeHandle.subscribe("SafeControllerService/Setpoint", 1, safeSetPointCallback);
-
- // subscriber for DBChanged
- ros::Subscriber DBChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, DBChangedCallback);
-
- // subscriber for emergencyStop
- ros::Subscriber emergencyStopSubscriber = namespaceNodeHandle.subscribe("/my_GUI/emergencyStop", 1, emergencyStopCallback);
-
- // Subscriber for "commandAllAgents" commands that are sent from the coordinator node
- ros::Subscriber commandAllAgentsSubscriber = namespaceNodeHandle.subscribe("/my_GUI/commandAllAgents", 1, commandAllAgentsCallback);
-
- // crazyradio status. Connected, connecting or disconnected
- ros::Subscriber crazyRadioStatusSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, crazyRadioStatusCallback);
-
- // Subscriber for "crazyRadioCommandAllAgents" commands that are sent from the coordinator node
- ros::Subscriber crazyRadioCommandAllAgentsSubscriber = namespaceNodeHandle.subscribe("/my_GUI/crazyRadioCommandAllAgents", 1, crazyRadioCommandAllAgentsCallback);
-
- // know the battery level of the CF
- ros::Subscriber CFBatterySubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CFBattery", 1, CFBatteryCallback);
-
- // INITIALISE THE BATTERY VOLTAGE TO SOMETHING CLOSE TO FULL
- // > This is used to prevent the "Low Battery" being trigged when the
- // first battery voltage data is received
- m_battery_voltage = 4.2f;
-
- //start with safe controller
- flying_state = STATE_MOTORS_OFF;
- setControllerUsed(SAFE_CONTROLLER);
- setInstantController(SAFE_CONTROLLER); //initialize this also, so we notify GUI
-
- setBatteryStateTo(BATTERY_STATE_NORMAL); //initialize battery state
-
- // Open a ROS "bag" to store data for post-analysis
- // std::string package_path;
- // package_path = ros::package::getPath("d_fall_pps") + "/";
- // ROS_INFO_STREAM(package_path);
- // std::string record_file = package_path + "LoggingPPSClient.bag";
- // bag.open(record_file, rosbag::bagmode::Write);
-
- ros::spin();
-
- // Close the ROS "bag" that was opened to store data for post-analysis
- //bag.close();
-
- return 0;
-}
diff --git a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
deleted file mode 100644
index 976274ba52c400debaed6918823a5f7e5c708d29..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ /dev/null
@@ -1,450 +0,0 @@
-// 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
-//
-// ----------------------------------------------------------------------------------
-
-
-
-
-
-// INCLUDE THE HEADER
-#include "nodes/ParameterService.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
-// ----------------------------------------------------------------------------------
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// 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
-// ----------------------------------------------------------------------------------
-
-
-
-
-void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
-{
- // Extract from the "msg" for which controller the YAML
- // parameters should be loaded
- int controller_to_load_yaml = msg.data;
-
- 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
- // from a YAML file
- bool isValidToAttemptLoad = true;
-
- // Instantiate a local variable for the 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");
-
- // Switch between loading for the different controllers
- // ----------------------------------------
- // 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_base_namespace + "/SafeController";
- }
- // ----------------------------------------
- // 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 demo controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/DemoController.yaml " + m_base_namespace + "/DemoController";
- }
- // ----------------------------------------
- // 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 demo controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/StudentController.yaml " + m_base_namespace + "/StudentController";
- }
- // ----------------------------------------
- // 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 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";
- }
- // ----------------------------------------
- // FOR THE PICKER CONTROLLER
- else if (
- ((controller_to_load_yaml==LOAD_YAML_PICKER_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
- ||
- ((controller_to_load_yaml==LOAD_YAML_PICKER_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
- )
- {
- // Re-load the parameters of the demo controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/PickerController.yaml " + m_base_namespace + "/PickerController";
- }
- else
- {
- // Let the user know that the command was not recognised
- 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;
- }
-
-
- // Only bother with ttempting to loaded the .yaml file, and subseuently send the "ready for fetch"
- // message if something can actually be loaded from a YAML file
- if (isValidToAttemptLoad)
- {
- // 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 );
-
- // 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
- system(cmd.c_str());
-
- // Pause breifly to ensure that the yaml file is fully loaded
- ros::Duration(0.5).sleep();
-
- // Instantiate a local varaible to confirm that something was actually loaded from
- // a YAML file
- bool isReadyForFetch = true;
-
- // Instantiate a local variable for the fetch message
- std_msgs::Int32 fetch_msg;
- // Fill in the data of the fetch message
- 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)
- {
- controllerYamlReadyForFetchPublisher.publish(fetch_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;
-}
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// 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
-// ----------------------------------------------------------------------------------
-
-int main(int argc, char* argv[])
-{
- // Starting the ROS-node
- ros::init(argc, argv, "ParameterService");
-
- // 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 "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("[PARAMETER SERVICE] Failed to get type from ParameterService");
- }
-
- // Set the "my_type" instance variable based on this string loaded
- if ((!type_string.compare("coordinator")))
- {
- my_type = TYPE_COORDINATOR;
- }
- else if ((!type_string.compare("agent")))
- {
- my_type = TYPE_AGENT;
- }
- else
- {
- // Set "my_type" to the value indicating that it is invlid
- my_type = TYPE_INVALID;
- ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
- }
-
-
- // Get the value of the "agentID" parameter into the instance variable "my_agentID"
- if(!nodeHandle.getParam("agentID", my_agentID))
- {
- // Throw an error if the agent ID parameter could not be obtained
- 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
- controllerYamlReadyForFetchPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerYamlReadyForFetch", 1);
-
-
- // Construct the string to the namespace of this Paramater Service
- switch (my_type)
- {
- case TYPE_AGENT:
- {
- //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;
- }
-
- // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
- // > The master GUI
- case TYPE_COORDINATOR:
- {
- //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("[PARAMETER SERVICE] The 'my_type' type parameter was not recognised.");
- break;
- }
- }
-
-
-
-
- // SUBSCRIBE TO THE APPROPRIATE "request" MESSAGES DEPENDING ON THE "my_type"
- // Delare the subscribers as local variables here so that they persist for the life
- // time of this main() function. The varaibles will be assigned in the switch case below
- // > Subscribers for when this Parameter Service node is: TYPE_AGENT
- ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
- ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_coordinator;
- // > Subscribers for when this Parameter Service node is: TYPE_COORDINATOR
- ros::Subscriber requestLoadControllerYamlSubscriber_coordinator_to_self;
-
- // SUBSCRIBE TO THE APPROPRIATE "request" MESSAGES DEPENDING ON THE "my_type"
- switch (my_type)
- {
- // AN AGENT TYPE PARAMETER SERVICE IS REQUESTED FROM:
- // > The master GUI
- // > The agent's own "PPSClient" node
- case TYPE_AGENT:
- {
- // Subscribing to the agent's own PPSclient
- // > First: Construct a node handle to the PPSclient
- ros::NodeHandle nh_PPSClient_for_this_agent("PPSClient");
- // > Second: Subscribe to the "requestLoadControllerYaml" topic
- requestLoadControllerYamlSubscriber_agent_to_self = nh_PPSClient_for_this_agent.subscribe("requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
-
- // Subscribing to the coordinator
- // > First: construct a node handle to the coordinator
- ros::NodeHandle nh_coordinator_for_this_agent = ros::NodeHandle();
- // > Second: Subscribe to the "requestLoadControllerYaml" topic
- 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("[PARAMETER SERVICE] Subscribed to 'requestLoadControllerYaml' messages from both the 'my_GUI' and the 'PPSClient'");
- break;
- }
-
- // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
- // > The master GUI
- case TYPE_COORDINATOR:
- {
- // Subscribing to the coordinator's own "my_GUI"
- // > First: Get the node handle required
- ros::NodeHandle nh_coordinator_for_this_coordinator = ros::NodeHandle();
- // > Second: Subscribe to requests from: the master GUI
- 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("[PARAMETER SERVICE] Subscribed to 'requestLoadControllerYaml' messages from 'my_GUI'");
- break;
- }
-
- default:
- {
- ROS_ERROR("[PARAMETER SERVICE] The 'my_type' type parameter was not recognised.");
- break;
- }
- }
-
-
- // 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/PickerControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
deleted file mode 100644
index 273d4a7f1441b679a2c6a294149a3a0c688b6643..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/src/nodes/PickerControllerService.cpp
+++ /dev/null
@@ -1,1703 +0,0 @@
-// 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/PickerControllerService.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
-// ----------------------------------------------------------------------------------
-
-
-
-
-// REMINDER OF THE NAME OF USEFUL CLASS VARIABLE
-// // > Mass of the Crazyflie quad-rotor, in [grams]
-// float m_mass_CF_grams;
-// // > Mass of the letters to be lifted, in [grams]
-// float m_mass_E_grams;
-// float m_mass_T_grams;
-// float m_mass_H_grams;
-// // > Total mass of the Crazyflie plus whatever it is carrying, in [grams]
-// float m_mass_total_grams;
-// // Thickness of the object at pick-up and put-down, in [meters]
-// // > This should also account for extra height due to
-// // the surface where the object is
-// float m_thickness_of_object_at_pickup;
-// float m_thickness_of_object_at_putdown;
-// // (x,y) coordinates of the pickup location
-// std::vector<float> m_pickup_coordinates_xy(2);
-// // (x,y) coordinates of the drop off location
-// std::vector<float> m_dropoff_coordinates_xy_for_E(2);
-// std::vector<float> m_dropoff_coordinates_xy_for_T(2);
-// std::vector<float> m_dropoff_coordinates_xy_for_H(2);
-// // Length of the string from the Crazyflie
-// // to the end of the Picker, in [meters]
-// float m_picker_string_length;
-// // > The setpoints for (x,y,z) position and yaw angle, in that order
-// float m_setpoint[4] = {0.0,0.0,0.4,0.0};
-// float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};
-// // > Small adjustments to the x-y setpoint
-// float m_xAdjustment = 0.0f;
-// float m_yAdjustment = 0.0f;
-// // Boolean for whether to limit rate of change of the setpoint
-// bool m_shouldSmoothSetpointChanges = true;
-// // Max setpoint change per second
-// float m_max_setpoint_change_per_second_horizontal;
-// float m_max_setpoint_change_per_second_vertical;
-// float m_max_setpoint_change_per_second_yaw_degrees;
-// float m_max_setpoint_change_per_second_yaw_radians;
-// // Frequency at which the controller is running
-// float m_vicon_frequency;
-
-
-// A FEW EXTRA COMMENTS ABOUT THE MOST IMPORTANT VARIABLES
-
-// Variable name: m_setpoint
-// Description:
-// This is a float array of length 4. It specifies a location
-// in space where you want the drone to be. The 4 element are:
-// >> m_setpoint[0] The x-poistion in [meters]
-// >> m_setpoint[1] The y-poistion in [meters]
-// >> m_setpoint[2] The z-poistion in [meters]
-// >> m_setpoint[3] The yaw heading angle in [radians]
-
-
-// Variable name: m_setpoint_for_controller
-// Description:
-// Similar to the variable "m_setpoint" this is also float array
-// of length 4 that specifies an (x,y,z,yaw) location. The
-// difference it that this variable specifies the location where
-// the low-level controller is guiding the drone to be.
-// HINT: to make changes the "m_setpoint" variable, you can edit
-// the function named "perControlCycleOperations" so that the
-// "m_setpoint_for_controller" changes by a maximum amount at
-// each cycle of the contoller
-
-
-
-// THIS FUNCTION IS CALLED AT "m_vicon_frequency" HERTZ.
-// IT CAN BE USED TO ADJUST THINGS IN "REAL TIME".
-// For example, the equation:
-// >> m_max_setpoint_change_per_second_horizontal / m_vicon_frequency
-// will convert the "change per second" to a "change per cycle".
-
-void perControlCycleOperations()
-{
- if (m_shouldSmoothSetpointChanges)
- {
- for(int i = 0; i < 4; ++i)
- {
- float max_for_this_coordinate;
- // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
- switch (i)
- {
- case 0:
- max_for_this_coordinate = m_max_setpoint_change_per_second_horizontal / m_vicon_frequency;
- break;
- case 1:
- max_for_this_coordinate = m_max_setpoint_change_per_second_horizontal / m_vicon_frequency;
- break;
- case 2:
- max_for_this_coordinate = m_max_setpoint_change_per_second_vertical / m_vicon_frequency;
- break;
- case 3:
- max_for_this_coordinate = m_max_setpoint_change_per_second_yaw_radians / m_vicon_frequency;
- break;
- // Handle the exception
- default:
- max_for_this_coordinate = 0.0f;
- break;
- }
-
- // Compute the difference in setpoint
- float setpoint_difference = m_setpoint[i] - m_setpoint_for_controller[i];
-
- // Clip the difference to the maximum
- if (setpoint_difference > max_for_this_coordinate)
- {
- setpoint_difference = max_for_this_coordinate;
- }
- else if (setpoint_difference < -max_for_this_coordinate)
- {
- setpoint_difference = -max_for_this_coordinate;
- }
-
- // Update the setpoint of the controller
- m_setpoint_for_controller[i] += setpoint_difference;
- }
-
- }
- else
- {
- m_setpoint_for_controller[0] = m_setpoint[0];
- m_setpoint_for_controller[1] = m_setpoint[1];
- m_setpoint_for_controller[2] = m_setpoint[2];
- m_setpoint_for_controller[3] = m_setpoint[3];
- }
-}
-
-
-
-
-
-
-
-// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
-
-void buttonPressed_gotoStart()
-{
- ROS_INFO("[PICKER CONTROLLER] Goto Start button pressed");
-
- // The drone should move smoothly to the start point:
- m_shouldSmoothSetpointChanges = true;
- // Set the (x,y) coordinates for the start point:
- m_setpoint[0] = m_pickup_coordinates_xy[0];
- m_setpoint[1] = m_pickup_coordinates_xy[1];
- // Set the z coordinate to be a little more than the
- // length of the "picker string"
- m_setpoint[2] = m_picker_string_length + 0.10;
- // Publish the setpoint so that the GUI updates
- publishCurrentSetpoint();
-}
-
-void buttonPressed_attach()
-{
- ROS_INFO("[PICKER CONTROLLER] Attach button pressed");
-
- m_shouldSmoothSetpointChanges = true;
- m_setpoint[2] = m_picker_string_length + m_thickness_of_object_at_pickup;
- publishCurrentSetpoint();
-}
-
-void buttonPressed_pickup()
-{
- ROS_INFO("[PICKER CONTROLLER] Pick up button pressed");
-
- m_shouldSmoothSetpointChanges = true;
- m_mass_total_grams = m_mass_CF_grams + m_mass_E_grams;
- m_setpoint[2] = m_picker_string_length + 0.25;
- publishCurrentSetpoint();
-}
-
-void buttonPressed_gotoEnd()
-{
- ROS_INFO("[PICKER CONTROLLER] Goto End button pressed");
-
- m_shouldSmoothSetpointChanges = true;
- m_setpoint[0] = m_dropoff_coordinates_xy_for_E[0];
- m_setpoint[1] = m_dropoff_coordinates_xy_for_E[1];
- publishCurrentSetpoint();
-}
-
-void buttonPressed_putdown()
-{
- ROS_INFO("[PICKER CONTROLLER] Put down button pressed");
-
- m_shouldSmoothSetpointChanges = true;
- m_setpoint[2] = m_picker_string_length + m_thickness_of_object_at_putdown;
- m_mass_total_grams = m_mass_CF_grams;
- publishCurrentSetpoint();
-}
-
-void buttonPressed_squat()
-{
- ROS_INFO("[PICKER CONTROLLER] Squat button pressed");
-
- m_shouldSmoothSetpointChanges = true;
- m_setpoint[2] = m_picker_string_length - 0.10;
- m_mass_total_grams = m_mass_CF_grams;
- publishCurrentSetpoint();
-}
-
-void buttonPressed_jump()
-{
- ROS_INFO("[PICKER CONTROLLER] Jump button pressed");
-
- m_shouldSmoothSetpointChanges = false;
- m_setpoint[2] = m_picker_string_length + 0.10;
- m_mass_total_grams = m_mass_CF_grams;
- publishCurrentSetpoint();
-}
-
-
-
-// THESE CALLBACK FUNCTIONS ALLOW YOU TO IMPLEMENT SOME
-// CUSTOM ACTION IN RESPONSE TO THE RESPECTIVE BUTTON PRESSES
-
-void buttonPressed_1()
-{
- ROS_INFO("[PICKER CONTROLLER] Button 1 pressed");
-}
-void buttonPressed_2()
-{
- ROS_INFO("[PICKER CONTROLLER] Button 2 pressed");
-}
-
-void buttonPressed_3()
-{
- ROS_INFO("[PICKER CONTROLLER] Button 3 pressed");
-}
-
-void buttonPressed_4()
-{
- ROS_INFO("[PICKER CONTROLLER] Button 4 pressed");
-}
-
-
-
-
-
-
-
-
-
-
-void zSetpointCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is z-height in [meters]
- float z_height = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] Z Slider changed to " << z_height << " [m]" );
- // Update the z-component of the setpoint class variable
- m_setpoint[2] = z_height;
-}
-
-
-void yawSetpointCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is yaw-angle in [radians]
- float yaw_angle = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] Yaw Dial changed to " << (yaw_angle*RAD2DEG) << " [deg]" );
- // Update the yaw-component of the setpoint class variable
- m_setpoint[3] = yaw_angle;
-}
-
-void massCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is mass in [grams]
- float mass = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] Mass slider changed to " << mass << " [grams]" );
- // Update the total mass class variable
- m_mass_total_grams = mass;
-}
-
-void xAdjustmentCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is adjustment in [meters]
- float x_adjustment = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] X adjustment slider changed to " << x_adjustment << " [m]" );
- // Update the x-adjustment class variable
- m_xAdjustment = x_adjustment;
-}
-
-void yAdjustmentCallback(const std_msgs::Float32& msg)
-{
- // The "data" in the message is adjustment in [meters]
- float y_adjustment = msg.data;
- // Display the data
- ROS_INFO_STREAM("[PICKER CONTROLLER] Y adjustment slider changed to " << y_adjustment << " [m]" );
- // Update the y-adjustment class variable
- m_yAdjustment = y_adjustment;
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-// CALLBACK FUNCTION THAT RUN WHEN THE RESPECTIVE BUTTON IS PRESSED
-// > AND A SETPOINT IS PROVIDED
-
-void buttonPressedWithSetpoint_gotoStart(const SetpointV2& newSetpointV2)
-{
- ROS_INFO_STREAM("[PICKER CONTROLLER] Goto Start button pressed: (x,y,z)=(" << newSetpointV2.x << "," << newSetpointV2.y << "," << newSetpointV2.z << "), smooth=" << int(newSetpointV2.isChecked) );
-
- // Use the boolean for the smoothing flag
- m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
- // Set the (x,y,z) coordinates:
- m_setpoint[0] = newSetpointV2.x;
- m_setpoint[1] = newSetpointV2.y;
- m_setpoint[2] = newSetpointV2.z;
- // Publish the setpoint so that the GUI updates
- //publishCurrentSetpoint();
-}
-
-void buttonPressedWithSetpoint_attach(const SetpointV2& newSetpointV2)
-{
- ROS_INFO_STREAM("[PICKER CONTROLLER] Attach button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // Use the boolean for the smoothing flag
- m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
- // Set the z coordinate:
- m_setpoint[2] = newSetpointV2.z;
- // Publish the setpoint so that the GUI updates
- //publishCurrentSetpoint();
-}
-
-void buttonPressedWithSetpoint_pickup(const SetpointV2& newSetpointV2)
-{
- ROS_INFO_STREAM("[PICKER CONTROLLER] Pick up button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // Use the boolean for the smoothing flag
- m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
- // Set the z coordinate:
- m_setpoint[2] = newSetpointV2.z;
- // Update the mass of the Crazyflie
- m_mass_total_grams = m_mass_CF_grams + m_mass_E_grams;
- // Publish the setpoint so that the GUI updates
- //publishCurrentSetpoint();
-}
-
-void buttonPressedWithSetpoint_gotoEnd(const SetpointV2& newSetpointV2)
-{
- ROS_INFO_STREAM("[PICKER CONTROLLER] Goto End button pressed: (x,y)=(" << newSetpointV2.x << "," << newSetpointV2.y << "), smooth=" << int(newSetpointV2.isChecked) );
-
- // Use the boolean for the smoothing flag
- m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
- // Set the (x,y) coordinates:
- m_setpoint[0] = newSetpointV2.x;
- m_setpoint[1] = newSetpointV2.y;
- // Publish the setpoint so that the GUI updates
- //publishCurrentSetpoint();
-}
-
-void buttonPressedWithSetpoint_putdown(const SetpointV2& newSetpointV2)
-{
- ROS_INFO_STREAM("[PICKER CONTROLLER] Put down button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // Use the boolean for the smoothing flag
- m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
- // Set the z coordinate:
- m_setpoint[2] = newSetpointV2.z;
- // Update the mass of the Crazyflie
- m_mass_total_grams = m_mass_CF_grams;
- // Publish the setpoint so that the GUI updates
- //publishCurrentSetpoint();
-}
-
-void buttonPressedWithSetpoint_squat(const SetpointV2& newSetpointV2)
-{
- ROS_INFO_STREAM("[PICKER CONTROLLER] Squat button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // Use the boolean for the smoothing flag
- m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
- // Set the z coordinate:
- m_setpoint[2] = newSetpointV2.z;
- // Update the mass of the Crazyflie
- m_mass_total_grams = m_mass_CF_grams;
- // Publish the setpoint so that the GUI updates
- //publishCurrentSetpoint();
-}
-
-void buttonPressedWithSetpoint_jump(const SetpointV2& newSetpointV2)
-{
- ROS_INFO_STREAM("[PICKER CONTROLLER] Jump button pressed: z=" << newSetpointV2.z << ", smooth=" << int(newSetpointV2.isChecked) );
-
- // Use the boolean for the smoothing flag
- m_shouldSmoothSetpointChanges = newSetpointV2.isChecked;
- // Set the z coordinate:
- m_setpoint[2] = newSetpointV2.z;
- // Update the mass of the Crazyflie
- m_mass_total_grams = m_mass_CF_grams;
- // Publish the setpoint so that the GUI updates
- //publishCurrentSetpoint();
-}
-
-
-void buttonPressedWithSetpoint_1(const SetpointV2& newSetpointV2)
-{
- ROS_INFO("[PICKER CONTROLLER] Button 1 pressed with a setpoint provided");
-}
-
-void buttonPressedWithSetpoint_2(const SetpointV2& newSetpointV2)
-{
- ROS_INFO("[PICKER CONTROLLER] Button 2 pressed with a setpoint provided");
-}
-
-void buttonPressedWithSetpoint_3(const SetpointV2& newSetpointV2)
-{
- ROS_INFO("[PICKER CONTROLLER] Button 3 pressed with a setpoint provided");
-}
-
-void buttonPressedWithSetpoint_4(const SetpointV2& newSetpointV2)
-{
- ROS_INFO("[PICKER CONTROLLER] Button 4 pressed with a setpoint provided with a setpoint provided");
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-// ------------------------------------------------------------------------------
-// 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 "PickerController" 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)
-{
-
- // Keep track of time
- m_time_ticks++;
- m_time_seconds = float(m_time_ticks) / m_vicon_frequency;
-
-
- // CALL THE FUNCTION FOR PER CYLCE OPERATIONS
- perControlCycleOperations();
-
- // 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,m_setpoint_for_controller,current_bodyFrameError);
-
-
-
- // CARRY OUT THE CONTROLLER COMPUTATIONS
- // Call the function that performs the control computations for this mode
- calculateControlOutput_viaLQRforRates(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("[PICKER CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'PickerControllerService': 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_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;
- // > Compute the feed-forward force
- float feed_forward_thrust_per_motor = m_mass_total_grams * 9.81/(1000*4);
- // > Put in the per motor commands
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
-
-
- // 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);
- }
-}
-
-
-
-// ***********************************************************
-// 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
-
-void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response)
-{
-
- // 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
-// ------------------------------------------------------------------------------
-
-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] - m_xAdjustment;
- stateInertial[1] = stateInertial[1] - setpoint[1] - m_yAdjustment;
- stateInertial[2] = stateInertial[2] - setpoint[2];
-
- if (stateInertial[2] > 40.0f)
- {
- stateInertial[2] = 40.0f;
- }
- else if (stateInertial[2] < -40.0f)
- {
- stateInertial[2] = -40.0f;
- }
-
- // 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)
-{
- m_setpoint[0] = newSetpoint.x;
- m_setpoint[1] = newSetpoint.y;
- m_setpoint[2] = newSetpoint.z;
- m_setpoint[3] = newSetpoint.yaw;
-}
-
-
-void publishCurrentSetpoint()
-{
- Setpoint msg_setpoint;
- msg_setpoint.x = m_setpoint[0];
- msg_setpoint.y = m_setpoint[1];
- msg_setpoint.z = m_setpoint[2];
- msg_setpoint.yaw = m_setpoint[3];
-
- pickerSetpointToGUIPublisher.publish(msg_setpoint);
-}
-
-
-// ----------------------------------------------------------------------------------
-// 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 buttonPressedCallback(const std_msgs::Int32& msg)
-{
- // Extract the data from the message
- int button_index = msg.data;
-
- // Switch between the button pressed
- switch(button_index)
- {
- case PICKER_BUTTON_GOTOSTART:
- buttonPressed_gotoStart();
- break;
- case PICKER_BUTTON_ATTACH:
- buttonPressed_attach();
- break;
- case PICKER_BUTTON_PICKUP:
- buttonPressed_pickup();
- break;
- case PICKER_BUTTON_GOTOEND:
- buttonPressed_gotoEnd();
- break;
- case PICKER_BUTTON_PUTDOWN:
- buttonPressed_putdown();
- break;
- case PICKER_BUTTON_SQUAT:
- buttonPressed_squat();
- break;
- case PICKER_BUTTON_JUMP:
- buttonPressed_jump();
- break;
- case PICKER_BUTTON_1:
- buttonPressed_1();
- break;
- case PICKER_BUTTON_2:
- buttonPressed_2();
- break;
- case PICKER_BUTTON_3:
- buttonPressed_3();
- break;
- case PICKER_BUTTON_4:
- buttonPressed_4();
- break;
- default:
- {
- // Let the user know that the command was not recognised
- ROS_INFO_STREAM("[PICKER CONTROLLER] A button pressed message was received in the controller but not recognised, message.data = " << button_index );
- break;
- }
- }
-}
-
-
-
-void buttonPressedWithSetpointCallback(const SetpointV2& newSetpointV2)
-{
- // Extract the "buttonID" from the message
- int button_index = newSetpointV2.buttonID;
-
- // Switch between the button pressed
- switch(button_index)
- {
- case PICKER_BUTTON_GOTOSTART:
- buttonPressedWithSetpoint_gotoStart(newSetpointV2);
- break;
- case PICKER_BUTTON_ATTACH:
- buttonPressedWithSetpoint_attach(newSetpointV2);
- break;
- case PICKER_BUTTON_PICKUP:
- buttonPressedWithSetpoint_pickup(newSetpointV2);
- break;
- case PICKER_BUTTON_GOTOEND:
- buttonPressedWithSetpoint_gotoEnd(newSetpointV2);
- break;
- case PICKER_BUTTON_PUTDOWN:
- buttonPressedWithSetpoint_putdown(newSetpointV2);
- break;
- case PICKER_BUTTON_SQUAT:
- buttonPressedWithSetpoint_squat(newSetpointV2);
- break;
- case PICKER_BUTTON_JUMP:
- buttonPressedWithSetpoint_jump(newSetpointV2);
- break;
- case PICKER_BUTTON_1:
- buttonPressedWithSetpoint_1(newSetpointV2);
- break;
- case PICKER_BUTTON_2:
- buttonPressedWithSetpoint_2(newSetpointV2);
- break;
- case PICKER_BUTTON_3:
- buttonPressedWithSetpoint_3(newSetpointV2);
- break;
- case PICKER_BUTTON_4:
- buttonPressedWithSetpoint_4(newSetpointV2);
- break;
- default:
- {
- // Let the user know that the command was not recognised
- ROS_INFO_STREAM("[PICKER CONTROLLER] A button pressed message was received in the controller but not recognised, message.data = " << button_index );
- 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_PICKER_CONTROLLER_FROM_OWN_AGENT:
- {
- // Let the user know that this message was received
- ROS_INFO("[PICKER 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_PICKER_CONTROLLER_FROM_COORDINATOR:
- {
- // Let the user know that this message was received
- ROS_INFO("[PICKER 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 PickerControllerService 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 PickerController.yaml file
-
- // Add the "PickerController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_pickerController(nodeHandle, "PickerController");
-
- // > The mass of the crazyflie
- m_mass_CF_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_CF");
-
- // > The mass of the letters
- m_mass_E_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_E");
- m_mass_T_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_T");
- m_mass_H_grams = getParameterFloat(nodeHandle_for_pickerController , "mass_H");
-
- // Thickness of the object at pick-up and put-down, in [meters]
- // > This should also account for extra height due to the surface where the object is
- m_thickness_of_object_at_pickup = getParameterFloat(nodeHandle_for_pickerController , "thickness_of_object_at_pickup");
- m_thickness_of_object_at_putdown = getParameterFloat(nodeHandle_for_pickerController , "thickness_of_object_at_putdown");
-
- // (x,y) coordinates of the pickup location
- getParameterFloatVector(nodeHandle_for_pickerController, "pickup_coordinates_xy", m_pickup_coordinates_xy, 2);
-
- // (x,y) coordinates of the drop off location
- getParameterFloatVector(nodeHandle_for_pickerController, "dropoff_coordinates_xy_for_E", m_dropoff_coordinates_xy_for_E, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "dropoff_coordinates_xy_for_T", m_dropoff_coordinates_xy_for_T, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "dropoff_coordinates_xy_for_H", m_dropoff_coordinates_xy_for_H, 2);
-
- // Length of the string from the Crazyflie to the end of the Picker, in [meters]
- m_picker_string_length = getParameterFloat(nodeHandle_for_pickerController , "picker_string_length");
-
- // Max setpoint change per second
- m_max_setpoint_change_per_second_horizontal = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_horizontal");
- m_max_setpoint_change_per_second_vertical = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_vertical");
- m_max_setpoint_change_per_second_yaw_degrees = getParameterFloat(nodeHandle_for_pickerController , "max_setpoint_change_per_second_yaw_degrees");
-
- // THE FOLLOWING PARAMETERS ARE USED FOR THE LOW-LEVEL CONTROLLER
-
- // > The frequency at which the "computeControlOutput" is being called, as determined
- // by the frequency at which the Vicon system provides position and attitude data
- m_vicon_frequency = getParameterFloat(nodeHandle_for_pickerController, "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_pickerController, "control_frequency");
-
- // > The co-efficients of the quadratic conversation from 16-bit motor command to
- // thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_pickerController, "motorPoly", motorPoly, 3);
-
- // > The boolean for whether to execute the convert into body frame function
- shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_pickerController, "shouldPerformConvertIntoBodyFrame");
-
- // > The boolean indicating whether the (x,y,z,yaw) of this agent should be published
- // or not
- shouldPublishCurrent_xyz_yaw = getParameterBool(nodeHandle_for_pickerController, "shouldPublishCurrent_xyz_yaw");
-
- // Boolean indiciating whether the "Debug Message" of this agent should be published or not
- shouldPublishDebugMessage = getParameterBool(nodeHandle_for_pickerController, "shouldPublishDebugMessage");
-
- // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
- shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_pickerController, "shouldDisplayDebugInfo");
-
-
- // THE CONTROLLER GAINS:
-
- // A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_pickerController , "estimator_method" );
-
- // The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_pickerController, "gainMatrixYawRate", gainMatrixYawRate, 9);
-
- // 16-BIT COMMAND LIMITS
- cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_pickerController, "command_sixteenbit_min");
- cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_pickerController, "command_sixteenbit_max");
-
-
- // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
- // > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
- // > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_pickerController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
-
-
- // DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[PICKER CONTROLLER] DEBUGGING: the fetched PickerController/mass_CF = " << m_mass_CF_grams);
-
- // 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()
-{
- // Convert from degrees to radians
- m_max_setpoint_change_per_second_yaw_radians = DEG2RAD * m_max_setpoint_change_per_second_yaw_degrees;
-
- // Set that the estimator frequency is the same as the control frequency
- estimator_frequency = m_vicon_frequency;
-}
-
-
-
-
-// ----------------------------------------------------------------------------------
-// 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, "PickerControllerService");
-
- // 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 "PickerControllerService" node
- std::string m_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("[PICKER 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("[PICKER 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("[PICKER CONTROLLER] the namespace strings for accessing the Paramter Services are:");
- ROS_INFO_STREAM("[PICKER CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
- ROS_INFO_STREAM("[PICKER 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);
-
- // Initialise the total mass to be just the crazyflie
- m_mass_total_grams = m_mass_CF_grams;
-
- // *********************************************************************************
-
-
-
- // 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 "PickerController". 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("PickerController", 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 buttonPressedCallbackSubscriber = nodeHandle.subscribe("GUIButton", 1, customCommandReceivedCallback);
-
-
-
- // ADDED FOR THE PICKER
- ros::Subscriber pickerButtonPressedSubscriber = nodeHandle.subscribe("ButtonPressed", 1, buttonPressedCallback);
- ros::Subscriber pickerZSetpointSubscriber = nodeHandle.subscribe("ZSetpoint", 1, zSetpointCallback);
- ros::Subscriber pickerYawSetpointSubscriber = nodeHandle.subscribe("YawSetpoint", 1, yawSetpointCallback);
- ros::Subscriber pickerMassSubscriber = nodeHandle.subscribe("Mass", 1, massCallback);
- ros::Subscriber pickerXAdjustmentSubscriber = nodeHandle.subscribe("XAdjustment", 1, xAdjustmentCallback);
- ros::Subscriber pickerYAdjustmentSubscriber = nodeHandle.subscribe("YAdjustment", 1, yAdjustmentCallback);
-
- pickerSetpointToGUIPublisher = nodeHandle.advertise<Setpoint>("SetpointToGUI", 1);
-
- ros::Subscriber pickerButtonPressedWithSetpointSubscriber = nodeHandle.subscribe("ButtonPressedWithSetpoint", 1, buttonPressedWithSetpointCallback);
-
-
-
- // Print out some information to the user.
- ROS_INFO("[Picker 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/StudentControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
deleted file mode 100644
index 4ec1acc3bdb924d3196b80be9e3abd4db3e06d92..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ /dev/null
@@ -1,925 +0,0 @@
-// 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/StudentControllerService.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 "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)
-//
-// 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 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 stateErrorInertial[9];
-
- // Fill in the (x,y,z) position error
- stateErrorInertial[0] = request.ownCrazyflie.x - setpoint[0];
- stateErrorInertial[1] = request.ownCrazyflie.y - setpoint[1];
- stateErrorInertial[2] = request.ownCrazyflie.z - setpoint[2];
-
- // Compute an estimate of the velocity
- // > As simply the derivative between the current and previous position
- stateErrorInertial[3] = (stateErrorInertial[0] - previous_stateErrorInertial[0]) * control_frequency;
- stateErrorInertial[4] = (stateErrorInertial[1] - previous_stateErrorInertial[1]) * control_frequency;
- stateErrorInertial[5] = (stateErrorInertial[2] - previous_stateErrorInertial[2]) * control_frequency;
-
- // Fill in the roll and pitch angle measurements directly
- stateErrorInertial[6] = request.ownCrazyflie.roll;
- stateErrorInertial[7] = request.ownCrazyflie.pitch;
-
- // 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 = request.ownCrazyflie.yaw - 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
- stateErrorInertial[8] = yawError;
-
-
- // 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
- float stateErrorBody[9];
- convertIntoBodyFrame(stateErrorInertial, stateErrorBody, request.ownCrazyflie.yaw);
-
-
- // SAVE THE STATE ERROR TO BE USED NEXT TIME THIS FUNCTION IS CALLED
- // > as we have already used previous error we can now update it update it
- for(int i = 0; i < 9; ++i)
- {
- previous_stateErrorInertial[i] = stateErrorInertial[i];
- }
-
-
-
-
- // **********************
- // Y Y A W W
- // Y Y A A W W
- // Y A A W W
- // Y AAAAA W W W
- // Y A A W W
- //
- // YAW CONTROLLER
-
- // Instantiate the local variable for the yaw rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float yawRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the yaw rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
- }
-
- // Put the computed yaw rate into the "response" variable
- response.controlOutput.yaw = yawRate_forResponse;
-
-
-
-
- // **************************************
- // BBBB OOO DDDD Y Y ZZZZZ
- // B B O O D D Y Y Z
- // BBBB O O D D Y Z
- // B B O O D D Y Z
- // BBBB OOO DDDD Y ZZZZZ
- //
- // ALITUDE CONTROLLER (i.e., z-controller)
-
- // Instantiate the local variable for the thrust adjustment that will be
- // requested from the Crazyflie's on-baord "inner-loop" controller
- float thrustAdjustment = 0;
-
- // Perform the "-Kx" LQR computation for the thrust adjustment to respoond with
- for(int i = 0; i < 9; ++i)
- {
- thrustAdjustment -= gainMatrixThrust[i] * stateErrorBody[i];
- }
-
- // Put the computed thrust adjustment into the "response" variable,
- // as well as adding 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 "cf_weight_in_newtons" value is the total thrust required
- // as feed-forward. Assuming the the Crazyflie is symmetric, this
- // value is divided by four.
- float feed_forward_thrust_per_motor = cf_weight_in_newtons / 4.0;
- // > NOTE: the function "computeMotorPolyBackward" converts the input argument
- // from Newtons to the 16-bit command expected by the Crazyflie.
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
-
-
-
- // **************************************
- // BBBB OOO DDDD Y Y X X
- // B B O O D D Y Y X X
- // BBBB O O D D Y X
- // B B O O D D Y X X
- // BBBB OOO DDDD Y X X
- //
- // BODY FRAME X CONTROLLER
-
- // Instantiate the local variable for the pitch rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float pitchRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the pitch rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
- }
-
- // Put the computed pitch rate into the "response" variable
- response.controlOutput.pitch = pitchRate_forResponse;
-
-
-
-
- // **************************************
- // BBBB OOO DDDD Y Y Y Y
- // B B O O D D Y Y Y Y
- // BBBB O O D D Y Y
- // B B O O D D Y Y
- // BBBB OOO DDDD Y Y
- //
- // BODY FRAME Y CONTROLLER
-
- // Instantiate the local variable for the roll rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float rollRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the roll rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
- }
-
- // 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.
- it can either be Motor, Rate or Angle based */
- // response.controlOutput.onboardControllerType = MOTOR_MODE;
- response.controlOutput.onboardControllerType = RATE_MODE;
- // response.controlOutput.onboardControllerType = ANGLE_MODE;
-
-
-
-
-
- // ***********************************************************
- // 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);
-
- // An alternate debugging technique is to print out data directly to the
- // command line from which this node was launched.
-
- // 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-coordinates: " << request.ownCrazyflie.x);
- // ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
- // ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
- // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
- // ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
- // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
- // ROS_INFO_STREAM("Delta t: " << 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);
-
- // Return "true" to indicate that the control computation was performed successfully
- return true;
-}
-
-
-
-
-// ------------------------------------------------------------------------------
-// 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
-//
-// This function WILL NEED TO BE edited for successful completion of the PPS exercise
-void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
-{
- // 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];
-}
-
-
-
-
-
-// ------------------------------------------------------------------------------
-// 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;
-}
-
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// 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
-// 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_STUDENT_CONTROLLER_FROM_OWN_AGENT:
- {
- // Let the user know that this message was received
- 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
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
- break;
- }
-
- // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR:
- {
- // Let the user know that this message was received
- 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
- fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
- break;
- }
-
- default:
- {
- // Let the user know that the command was not relevant
- //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;
- }
- }
-}
-
-
-// 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 StudentController.yaml file
-
- // Add the "StudentController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_studentController(nodeHandle, "StudentController");
-
- // > The mass of the crazyflie
- cf_mass_in_grams = 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_in_grams );
-
- // > 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_studentController, "control_frequency");
-
- // > The co-efficients of the quadratic conversation from 16-bit motor command to
- // thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_studentController, "motorPoly", motorPoly, 3);
-
-
- // DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << cf_mass_in_grams);
-
- // 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]
- cf_weight_in_newtons = cf_mass_in_grams * 9.81/1000.0;
-
- // DEBUGGING: Print out one of the computed quantities
- ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << cf_weight_in_newtons);
-}
-
-
-
-// ----------------------------------------------------------------------------------
-// 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, "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
- // 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("[STUDENT 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("[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"
-
- // 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 "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("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:
- // "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("[STUDENT 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/srv/LoadYamlFiles.srv b/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
deleted file mode 100644
index f51e18525ad2cab1e31875c4c31014a98eda1d6a..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
+++ /dev/null
@@ -1,3 +0,0 @@
-string[] yamlFileNames
----
-float64 waitTime
diff --git a/wiki/faq.md b/wiki/faq.md
index 56da5fbf210369414c371478291df98119ab602d..04e4c9ba569d2aa82cc505c8cfdf238ffe76dd9c 100644
--- a/wiki/faq.md
+++ b/wiki/faq.md
@@ -35,9 +35,9 @@ git pull
The ``Student GUI`` can be launched from a terminal window with the following command:
```
-roslaunch d_fall_pps Student.launch
+roslaunch dfall_pkg Student.launch
```
-This can be run from any directory because ``d_fall_pps`` is defined as an environment variable that points to the absolute foler location ``~/work/D-FaLL-System/pps_ws/src/d_fall_pps/launch/``
+This can be run from any directory because ``dfall_pkg`` is defined as an environment variable that points to the absolute foler location ``~/work/D-FaLL-System/dfall_ws/src/dfall_pkg/launch/``
### How do I make changes to a ``*.cpp`` file take effect?
@@ -45,9 +45,9 @@ In essence you need to re-compile the code and re-launch all ROS nodes.
<b>Step 1)</b> Kill all ROS processes that are running nodes from the ``D-FaLL-System`` repository, i.e., you must kill the ``Student GUI`` node but you do not need to kill an ``rqt`` plotting node. To kill a process press ``Ctrl+c`` from the terminal window that is running the process.
-<b>Step 2)</b> In a terminal window, change to the ``pps_ws`` folder of the repository (where ``ws`` stands for workspace. On a machine setup as per the instructions this is:
+<b>Step 2)</b> In a terminal window, change to the ``dfall_ws`` folder of the repository (where ``ws`` stands for workspace. On a machine setup as per the instructions this is:
```
-cd ~work/D-FaLL-System/pps_ws/
+cd ~work/D-FaLL-System/dfall_ws/
```
<b>Step 3)</b> Compile the repository, which includes your changes, using the command:
```
@@ -79,7 +79,7 @@ This can happen for a variety of reasons, and generally relates to the local com
To understand how Step 5) could actually fix the problem, consider that ``~/.bashrc`` is run when a new terminal window is opened, and as part of installing the ``D-FaLL-System`` the following line is added to the ``.bashrc``:
``
-source <catkin workspace>/src/d_fall_pps/launch/Config.sh
+source <catkin workspace>/src/dfall_pkg/launch/Config.sh
``
And if you look at the ``Config.sh`` file in the repository you see that it defines environment variable relating to the ``ROS_MASTER_URI``, ``ROS_IP``, and ``ROS_NAMESPACE``. On occassion these are not properly defined on start up or are changed, hence closing and re-launching Terminal can resolve the problem.
@@ -91,9 +91,9 @@ The Vicon motion capture system provides position and attitude information via a
To check whether the Vicon Datastream software is properly added to the local computer, goto the [Installation](installation.md) wiki page and follow the instructions under the title "Vicon Datastream SDK Installation".
The main requirements are that:
-- The ``DataStreamClient.h`` header file needs to be located in ``~/pps_ws/src/d_fall_pps/lib/vicon/``,
-- The ``libViconDataStreamSDK_CPP.so`` shared object needs to be located in ``~/pps_ws/src/d_fall_pps/lib/vicon/``, and
-- A number of files of the form ``libboost_*`` should also be located in ``~/pps_ws/src/d_fall_pps/lib/vicon/``.
+- The ``DataStreamClient.h`` header file needs to be located in ``~/dfall_ws/src/dfall_pkg/lib/vicon/``,
+- The ``libViconDataStreamSDK_CPP.so`` shared object needs to be located in ``~/dfall_ws/src/dfall_pkg/lib/vicon/``, and
+- A number of files of the form ``libboost_*`` should also be located in ``~/dfall_ws/src/dfall_pkg/lib/vicon/``.
@@ -131,7 +131,7 @@ my_current_xyz_yaw_publisher = nodeHandle.advertise<Setpoint>("/<ID>/my_current_
```
where ``<ID>`` should be replaced by the ID of your computer, for example the number 7, ``my_current_xyz_yaw_topic`` is the name of the message topic that will be published, and ``Setpoint`` specifies the stucture of the message as defined in the file ``/msg/Setpoint.msg`` and included with:
```
-#include "d_fall_pps/Setpoint.h"
+#include "dfall_pkg/Setpoint.h"
```
added at the top of your ``StudentControllerService.cpp`` file.
diff --git a/wiki/installation.md b/wiki/installation.md
index 9d59218ea9a0847e2a06cc4e8cdd2a9cc9beae05..a97bfeebb14a6efef71a26f5982a3bf913380c67 100644
--- a/wiki/installation.md
+++ b/wiki/installation.md
@@ -5,7 +5,7 @@
### Install Script
Installation with the install script is the easiest. You will need:
- the install script
-- the ``d_fall_pps`` package compressed in a file called ``package.tar.gz``
+- the ``dfall_pkg`` package compressed in a file called ``package.tar.gz``
- the rule files for the USB connection to the crazyradio, called ``99-crazyflie.rules`` and ``99-crazyradio.rules``
These files all need to be in the same directory. To run the installation, move to the containing directory (pps\ install) and call it with
@@ -21,7 +21,7 @@ The installation process consists of the following steps:
The detailed instructions for the installation of ROS can be found [here](http://wiki.ros.org/kinetic/Installation/Ubuntu).
- Workspace: <br />
-Create a new catkin workspace and copy the ``d_fall_pps`` package into the ``src`` folder of the workspace. Then build the package with ``catkin_make`` called from the workspace root.
+Create a new catkin workspace and copy the ``dfall_pkg`` package into the ``src`` folder of the workspace. Then build the package with ``catkin_make`` called from the workspace root.
- Environment Setup: <br />
Add a new line in the ``/etc/hosts`` file that links the teacher's IP with the domain name ``teacher`` and create a file called ``/etc/StudentID`` that contains the student id. Only write digits without any other symbols or whitespace characters.
@@ -47,7 +47,7 @@ Add following lines to the bottom of the file ``~/.bashrc`` (replace ``<catkin w
```
source /opt/ros/kinetic/setup.bash
source <catkin workspace>/devel/setup.bash
-source <catkin workspace>/src/d_fall_pps/launch/Config.sh
+source <catkin workspace>/src/dfall_pkg/launch/Config.sh
```
The workspace setup script will only appear after the first compilation of the catkin workspace.
@@ -63,11 +63,11 @@ A Vicon motion capture system can be used to provide position and attitude measu
- De-compress the downloaded file and open the folder that corresponds to the latest ``Linux64-boost-1.xx.x`` version, when tested this was ``1.58.0``
- From the files found in this folder, copy the file ``DataStreamClient.h`` into the following folder:
```
-~/pps_ws/src/d_fall_pps/include/
+~/dfall_ws/src/dfall_pkg/include/
```
- Copy the file ``libViconDataStreamSDK_CPP.so`` into the following folder (noting that the folder ``lib`` and ``vicon`` may need to be created):
```
-~/pps_ws/src/d_fall_pps/lib/vicon/
+~/dfall_ws/src/dfall_pkg/lib/vicon/
```
- Copy all the files of the form ``libboost_*`` into the same ``/lib/vicon/`` folder
- Note that the ``DataStreamClient.h`` header file and the ``/lib/vicon/`` folder are in the ``.gitignore`` and hence will not be tracked or removed by the git repository (though some more advanced git commands may still remove them anyway).
@@ -78,7 +78,7 @@ A Vicon motion capture system can be used to provide position and attitude measu
### Removing Config.sh and replacing
As the teacher must not source the script ``Config.sh``, the ``.bashrc`` must be edited and the last line
```
-source ~/pps_ws/src/d_fall_pps/launch/Config.sh
+source ~/dfall_ws/src/dfall_pkg/launch/Config.sh
```
must be replaced with
```
@@ -109,7 +109,7 @@ doing so are these:
changes to take effect) <br><br>
2. After having put some reflective markers in the crazyflie, register it in the Vicon system with the name that you want. (in the PPS case, we call them PPS_CFXX)<br><br>
3. Go to the file channelLUT.cpp (path:
- `pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/channelLUT.cpp`) and add an
+ `dfall_ws/src/dfall_pkg/GUI_Qt/CrazyFlyGUI/src/channelLUT.cpp`) and add an
entry with the name of the crazyflie and the radio address. Follow the
formatting that the other entries have.<br><br>
<img src="./pics/LUT.png" style="width: 450px;"/> <br><br>
diff --git a/wiki/namespace_conventions.md b/wiki/namespace_conventions.md
index 7ba26326bc2fc5a3a08005c9d59d6da0e8395ac9..b47cd98191687b80f42af9afea93f1c470f7c362 100644
--- a/wiki/namespace_conventions.md
+++ b/wiki/namespace_conventions.md
@@ -13,7 +13,7 @@ echo $ROS_NAMESPACE
This environment vairable is set in the file:
```
-pps_ws/src/d_fall_pps/launch/Config.sh
+dfall_ws/src/dfall_pkg/launch/Config.sh
```
The environemnt variable is set by the following line in that file:
```
diff --git a/wiki/setup.md b/wiki/setup.md
index bcebb8ad20a7a9656d18345b6a2ab6a92b80a8d2..887bd01fd251c7b06e58a013b03eebb69ac67f5b 100644
--- a/wiki/setup.md
+++ b/wiki/setup.md
@@ -95,7 +95,7 @@ Then go to the _IPv4 Settings_ and choose **Manual** as the _Method_ and then ad
### Vicon, teacher and students
During installation process is the IP address of the teacher set to 10.42.0.10. (This value is written to the /etc/hosts file such that this IP address is accessible through the keyword _teacher_) <br>
-Have a look at `Config.sh` in `~/pps_ws/src/d_fall_pps/launch/`
+Have a look at `Config.sh` in `~/dfall_ws/src/dfall_pkg/launch/`
<br><img src="./pics/setup_pics/configsh2.png" style="width: 500px;"/> <br>
Here you see, that the ROS Master URI is set to be the teacher. This means that _roscore_ runs only on the teacher's computer. Your own IP address (_ROS IP_) is also set and taken from your Ethernet settings as defined in the section _Setting up the Vicon network_.
<br>
diff --git a/wiki/workflow_for_students.md b/wiki/workflow_for_students.md
index 114c154de9184293c9fc43ff88bbbc51911f01ea..b1cf0b1a0d61752d866c1c116b6c63259109dfba 100644
--- a/wiki/workflow_for_students.md
+++ b/wiki/workflow_for_students.md
@@ -11,7 +11,7 @@ gyrosensor needs to initialize.
* In the software side, everything has already been set up for the course, but
it would be helpful to check if the repository is in the last version, and
if the source code has been properly compiled. To do this, follow the next steps:
- 1. Go to the next folder: `cd ~/work/D-FaLL-System/pps_ws`
+ 1. Go to the next folder: `cd ~/work/D-FaLL-System/dfall_ws`
2. Checkout master branch of the repository and pull:<br />
``git checkout master``<br />
``git pull origin master``<br />
@@ -26,7 +26,7 @@ gyrosensor needs to initialize.
* Once all the prerequisites have been fulfilled, we can start the student's
GUI by going to a terminal and typing:
- `roslaunch d_fall_pps Student.launch`
+ `roslaunch dfall_pkg Student.launch`
*Note: for this to work, the teacher's computer has to be connected to the
network and the teacher's GUI has to be started before. Please wait until
@@ -69,7 +69,7 @@ gyrosensor needs to initialize.
safe controller to get familiar with the system.
*Note: there are different parameters in the file called
- `SafeController.yaml`, in the folder param (use `roscd d_fall_pps/param` in a
+ `SafeController.yaml`, in the folder param (use `roscd dfall_pkg/param` in a
terminal to go there).* **These are the safe controller parameters and should NOT be
changed.** *Take a look at the file and get familiar with the format used,
since may have to write your own for the student controller.*
@@ -81,12 +81,12 @@ important files that should be taken into account.
#### Files of interest:
-* In `d_fall_pps/src`
+* In `dfall_pkg/src`
* _StudentControllerService.cpp_ <br>
The file where students can implement their own controller. It provides already the ros service with the teacher. It can be used as a template.
-* In `d_fall_pps/param`
+* In `dfall_pkg/param`
* _ClientConfig.yaml_ <br><br>
<img src="./pics/client_config_yaml.png" style="width: 400px;"/> <br><br>
@@ -119,7 +119,7 @@ important files that should be taken into account.
*Sets the low battery thresholds at which the crazyflie will automatically
land. **Do not change these values.***
-* In `d_fall_pps/`
+* In `dfall_pkg/`
* CMakeLists.txt
@@ -131,7 +131,7 @@ important files that should be taken into account.
<!-- ##### -- Useful files: -->
-<!-- in `pps_ws/src/d_fall_pps/scripts` -->
+<!-- in `dfall_ws/src/dfall_pkg/scripts` -->
<!-- -\-> call scripts in terminal by going to the above path and then typing -->
<!-- ./SCRIPTNAME, e.g.: `./enable_crazyflie` -->
@@ -144,11 +144,11 @@ important files that should be taken into account.
<!-- ##### -- Files to look at: -->
-<!-- in `pps_ws/src/d_fall_pps/param` -->
+<!-- in `dfall_ws/src/dfall_pkg/param` -->
<!-- * _SafeController.yaml_ <br> -->
<!-- This file contains the control parameters that the SafeControllerService uses. The SafeControllerService loads this file when it starts. You might want to use a similar approach and can try to copy some functionality from SafeControllerService.cpp. -->
-<!-- in `pps_ws/scr/d_fall_pps/launch` <br> -->
+<!-- in `dfall_ws/scr/dfall_pkg/launch` <br> -->
<!-- The launch files contained in this directory are used to launch several nodes and some parameter files to be launched simultaneously. It is best, that you take a look at them yourself, but here is a brief explanation what the different launch files are for.<br> -->
<!-- To start the whole thing type the following in a terminal whilst being in the launch directory.<br> -->
<!-- `roslaunch filename.launch` -->
@@ -177,10 +177,10 @@ important files that should be taken into account.
without having to compile or restart the system. Here, as an example, we
pass some parameters that can be seen below:<br><br>
<img src="./pics/custom_controller_yaml.png" style="width: 400px;"/> <br><br>
-2. Go to `cd ~/work/D-FaLL-System/pps_ws` and write `catkin_make`.
+2. Go to `cd ~/work/D-FaLL-System/dfall_ws` and write `catkin_make`.
3. Once everything has compiled without errors, run the next launch file:
- `roslaunch d_fall_pps Student.launch`. This will run the student's GUI.
+ `roslaunch dfall_pkg Student.launch`. This will run the student's GUI.
4. Make sure that your Crazyflie is turned ON and connect to it. Choose the tab
called *Student Controller* and click on the button *Enable Student Controller*
@@ -218,7 +218,7 @@ to it*
vicon\_y,...). Additionally, there are up to 10 general purpose variables
that can be filled with any data we may be interested in plotting (value\_1,
value\_2,...., value\_10). <br><br>
-2. Once chosen the variables, save the file and go to `cd ~/work/D-FaLL-System/pps_ws` and write `catkin_make`.<br><br>
+2. Once chosen the variables, save the file and go to `cd ~/work/D-FaLL-System/dfall_ws` and write `catkin_make`.<br><br>
3. Open another terminal and type `rqt`. Then, in the top bar, go to
Plugins->Visualization->Plot. A new plot will be added to the screen. If you
want more than one plot, just add several ones doing the same thing. You will
@@ -231,7 +231,7 @@ to it*
`/<student_id>/StudentControllerService/DebugTopic/vicon_z`. You can see an
autocompletion of the
list of all the topics available when you start typing in the field "Topic". <br><br>
-5. Start the Student node following the steps mentioned before (`roslaunch d_fall_pps Student.launch`) and enable the Student Controller.<br><br>
+5. Start the Student node following the steps mentioned before (`roslaunch dfall_pkg Student.launch`) and enable the Student Controller.<br><br>
6. Once we are using the Student Controller, we will be seeing how the data
selected gets plotted in the rqt plots.
@@ -242,12 +242,12 @@ to it*
<!-- **Setup** -->
<!-- 1. Teacher must run his part, that publishes ViconData for students and hosts the roscore. -->
<!-- 2. Each student/group has a CrazyFlie and a laptop. -->
-<!-- 3. Use `roscd d_fall_pps/launch` in a terminal as well as `roscd d_fall_pps/scripts` in another terminal -->
+<!-- 3. Use `roscd dfall_pkg/launch` in a terminal as well as `roscd dfall_pkg/scripts` in another terminal -->
<!-- <br> -->
<!-- **Working** -->
<!-- 1. Adjust your custom controller -->
-<!-- 2. Use `catkin_make` in the pps_ws directory to compile your controller implementation -->
+<!-- 2. Use `catkin_make` in the dfall_ws directory to compile your controller implementation -->
<!-- 3. Start your crazyflie -->
<!-- 4. Launch the correct file in the launch directory as described above. ClientConfig.yaml has to be correct. -->
<!-- 5. Use the scripts to change from the safe to your custom controller. -->