diff --git a/dfall_ws/src/dfall_pkg/CMakeLists.txt b/dfall_ws/src/dfall_pkg/CMakeLists.txt
index c8aa8c4bc2067ce13485752083e57e9d973d5742..292aec970604a3f4086a8bdc1fec47eb5c0d6664 100755
--- a/dfall_ws/src/dfall_pkg/CMakeLists.txt
+++ b/dfall_ws/src/dfall_pkg/CMakeLists.txt
@@ -29,6 +29,7 @@ find_package(Qt5Widgets CONFIG)
find_package(Qt5Core CONFIG)
find_package(Qt5Gui CONFIG)
find_package(Qt5Svg CONFIG)
+find_package(Qt5Charts CONFIG)
@@ -81,9 +82,14 @@ if(Qt5Svg_FOUND)
else()
message(STATUS "NOTE: the Qt5 Svg package was NOT found")
endif()
+if(Qt5Charts_FOUND)
+ message(STATUS "NOTE: the Qt5 Charts package was found")
+else()
+ message(STATUS "NOTE: the Qt5 Charts package was NOT found")
+endif()
-if(Qt5Widgets_FOUND AND Qt5Core_FOUND AND Qt5Gui_FOUND AND Qt5Svg_FOUND)
+if(Qt5Widgets_FOUND AND Qt5Core_FOUND AND Qt5Gui_FOUND AND Qt5Svg_FOUND AND Qt5Charts_FOUND)
set(Qt5_FOUND TRUE)
else()
set(Qt5_FOUND FALSE)
@@ -163,9 +169,11 @@ if(Qt5_FOUND)
# be done manually
set(SRC_HDRS_QOBJECT_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_INC}/connectstartstopbar.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/controllerstatusbanner.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/controllertabs.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/coordinator.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/coordinatorrow.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/csonecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/defaultcontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/enablecontrollerloadyamlbar.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/mainwindow.h
@@ -181,9 +189,11 @@ if(Qt5_FOUND)
# Flying Agent GUI -- wrap UI file and QOBJECT files
qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/connectstartstopbar.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/controllerstatusbanner.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/controllertabs.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/coordinator.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/coordinatorrow.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/csonecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/defaultcontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/enablecontrollerloadyamlbar.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/mainwindow.ui
@@ -390,6 +400,8 @@ add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(CrazyRadioEmulator src/nodes/CrazyRadioEmulator.cpp
src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(CsoneControllerService src/nodes/CsoneControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
add_executable(DefaultControllerService src/nodes/DefaultControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
@@ -446,9 +458,11 @@ if(Qt5_FOUND)
${FLYING_AGENT_GUI_LIB_PATH_SRC}/main.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/rosNodeThread_for_flyingAgentGUI.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/connectstartstopbar.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/controllerstatusbanner.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/controllertabs.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/coordinator.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/coordinatorrow.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/csonecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/defaultcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/enablecontrollerloadyamlbar.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/pickercontrollertab.cpp
@@ -482,6 +496,7 @@ endif()
add_dependencies(FlyingAgentClient dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(BatteryMonitor dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CrazyRadioEmulator dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(CsoneControllerService 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})
@@ -535,6 +550,7 @@ endif()
target_link_libraries(FlyingAgentClient ${catkin_LIBRARIES})
target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
target_link_libraries(CrazyRadioEmulator ${catkin_LIBRARIES})
+target_link_libraries(CsoneControllerService ${catkin_LIBRARIES})
target_link_libraries(DefaultControllerService ${catkin_LIBRARIES})
target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
@@ -565,6 +581,7 @@ if(Qt5_FOUND)
# Flying Agent GUI -- link libraries
target_link_libraries(FlyingAgentGUI Qt5::Widgets) # GUI -- let FlyingAgentGUI have acesss to Qt stuff
+ target_link_libraries(FlyingAgentGUI Qt5::Charts)
target_link_libraries(FlyingAgentGUI ${catkin_LIBRARIES})
endif()
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
index daf227898c1ca5ea81aa9303b06e6dee3b202c71..42880b16bf944b158afc5ec23e3908806c19df32 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
@@ -10,6 +10,10 @@ greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
#greaterThan(QT_MAJOR_VERSION, 4): QT += svg
+greaterThan(QT_MAJOR_VERSION, 4): QT += charts
+
+#greaterThan(QT_MAJOR_VERSION, 4): QT += multimedia
+
TARGET = flyingAgentGUI
TEMPLATE = app
@@ -21,7 +25,9 @@ SOURCES += src/main.cpp\
src/topbanner.cpp \
src/connectstartstopbar.cpp \
src/enablecontrollerloadyamlbar.cpp \
+ src/controllerstatusbanner.cpp \
src/controllertabs.cpp \
+ src/csonecontrollertab.cpp \
src/safecontrollertab.cpp \
src/coordinator.cpp \
src/coordinatorrow.cpp \
@@ -37,7 +43,9 @@ HEADERS += include/mainwindow.h \
include/topbanner.h \
include/connectstartstopbar.h \
include/enablecontrollerloadyamlbar.h \
+ include/controllerstatusbanner.h \
include/controllertabs.h \
+ include/csonecontrollertab.h \
include/safecontrollertab.h \
include/coordinator.h \
include/coordinatorrow.h \
@@ -50,12 +58,13 @@ HEADERS += include/mainwindow.h \
include/remotecontrollertab.h
-
FORMS += forms/mainwindow.ui \
forms/topbanner.ui \
forms/connectstartstopbar.ui \
forms/enablecontrollerloadyamlbar.ui \
forms/controllertabs.ui \
+ forms/controllerstatusbanner.ui \
+ forms/csonecontrollertab.ui \
forms/safecontrollertab.ui \
forms/coordinator.ui \
forms/coordinatorrow.ui \
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllerstatusbanner.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllerstatusbanner.ui
new file mode 100644
index 0000000000000000000000000000000000000000..30abcf0ca82045e3d13b044e24280265f832e4b5
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllerstatusbanner.ui
@@ -0,0 +1,257 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>ControllerStatusBanner</class>
+ <widget class="QWidget" name="ControllerStatusBanner">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>173</width>
+ <height>1282</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <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="spacing">
+ <number>0</number>
+ </property>
+ <item row="0" column="0">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QFrame" name="frame_isOff">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:rgb(160,40,40);</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>15</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_isOff">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>O
+F
+F</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignHCenter|Qt::AlignTop</set>
+ </property>
+ <property name="wordWrap">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_isActive">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:rgb(40,120,40);</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>15</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_isActive">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>A
+C
+T
+I
+V
+E</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignHCenter|Qt::AlignTop</set>
+ </property>
+ <property name="wordWrap">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_isCoord">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:rgb(150,150,150)</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_4">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>15</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_isCoord">
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>C
+O
+O
+R
+D</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignHCenter|Qt::AlignTop</set>
+ </property>
+ <property name="wordWrap">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
index ceeabd07c08162f564703ebf09e27935f1c85ae4..aa69f2d57c1eb46019e2f15e29092508528766f9 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1150</width>
+ <width>1779</width>
<height>718</height>
</rect>
</property>
@@ -28,7 +28,7 @@
<item row="0" column="0">
<widget class="QTabWidget" name="controller_tabs_widget">
<property name="currentIndex">
- <number>4</number>
+ <number>6</number>
</property>
<property name="movable">
<bool>true</bool>
@@ -38,8 +38,59 @@
<string>Default</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_2">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ <property name="spacing">
+ <number>5</number>
+ </property>
+ <item row="0" column="1">
+ <widget class="QScrollArea" name="scrollArea_default">
+ <property name="widgetResizable">
+ <bool>true</bool>
+ </property>
+ <widget class="DefaultControllerTab" name="default_controller_tab_widget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1638</width>
+ <height>576</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
+ </item>
<item row="0" column="0">
- <widget class="DefaultControllerTab" name="default_controller_tab_widget" native="true"/>
+ <widget class="ControllerStatusBanner" name="statusBanner_default" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
</item>
</layout>
</widget>
@@ -48,8 +99,120 @@
<string>Student</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_4">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ <property name="spacing">
+ <number>5</number>
+ </property>
+ <item row="0" column="1">
+ <widget class="QScrollArea" name="scrollArea_student">
+ <property name="widgetResizable">
+ <bool>true</bool>
+ </property>
+ <widget class="StudentControllerTab" name="student_controller_tab_widget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1638</width>
+ <height>576</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
+ </item>
<item row="0" column="0">
- <widget class="StudentControllerTab" name="student_controller_tab_widget" native="true"/>
+ <widget class="ControllerStatusBanner" name="statusBanner_student" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="csone_tab">
+ <attribute name="title">
+ <string>CS1</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_8">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ <property name="spacing">
+ <number>5</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="ControllerStatusBanner" name="statusBanner_csone" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QScrollArea" name="scrollArea_csone">
+ <property name="widgetResizable">
+ <bool>true</bool>
+ </property>
+ <widget class="CsoneControllerTab" name="csone_controller_tab_widget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1638</width>
+ <height>576</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
</item>
</layout>
</widget>
@@ -58,8 +221,59 @@
<string>Picker</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_5">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ <property name="spacing">
+ <number>5</number>
+ </property>
+ <item row="0" column="1">
+ <widget class="QScrollArea" name="scrollArea_picker">
+ <property name="widgetResizable">
+ <bool>true</bool>
+ </property>
+ <widget class="PickerControllerTab" name="picker_controller_tab_widget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1638</width>
+ <height>576</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
+ </item>
<item row="0" column="0">
- <widget class="PickerControllerTab" name="picker_controller_tab_widget" native="true"/>
+ <widget class="ControllerStatusBanner" name="statusBanner_picker" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
</item>
</layout>
</widget>
@@ -68,8 +282,59 @@
<string>Tuning</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_6">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ <property name="spacing">
+ <number>5</number>
+ </property>
<item row="0" column="0">
- <widget class="TuningControllerTab" name="tuning_controller_tab_widget" native="true"/>
+ <widget class="ControllerStatusBanner" name="statusBanner_tuning" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QScrollArea" name="scrollArea_tuning">
+ <property name="widgetResizable">
+ <bool>true</bool>
+ </property>
+ <widget class="TuningControllerTab" name="tuning_controller_tab_widget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1638</width>
+ <height>576</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
</item>
</layout>
</widget>
@@ -78,8 +343,59 @@
<string>Remote</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_3">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ <property name="spacing">
+ <number>5</number>
+ </property>
+ <item row="0" column="1">
+ <widget class="QScrollArea" name="scrollArea_remote">
+ <property name="widgetResizable">
+ <bool>true</bool>
+ </property>
+ <widget class="RemoteControllerTab" name="remote_controller_tab_widget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1638</width>
+ <height>576</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
+ </item>
<item row="0" column="0">
- <widget class="RemoteControllerTab" name="remote_controller_tab_widget" native="true"/>
+ <widget class="ControllerStatusBanner" name="statusBanner_remote" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
</item>
</layout>
</widget>
@@ -88,8 +404,59 @@
<string>Template</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_7">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ <property name="spacing">
+ <number>5</number>
+ </property>
+ <item row="0" column="1">
+ <widget class="QScrollArea" name="scrollArea_template">
+ <property name="widgetResizable">
+ <bool>true</bool>
+ </property>
+ <widget class="TemplateControllerTab" name="template_controller_tab_widget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1638</width>
+ <height>576</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
+ </item>
<item row="0" column="0">
- <widget class="TemplateControllerTab" name="template_controller_tab_widget" native="true"/>
+ <widget class="ControllerStatusBanner" name="statusBanner_template" native="true">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>80</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>80</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
</item>
</layout>
</widget>
@@ -134,6 +501,18 @@
<header>remotecontrollertab.h</header>
<container>1</container>
</customwidget>
+ <customwidget>
+ <class>CsoneControllerTab</class>
+ <extends>QWidget</extends>
+ <header>csonecontrollertab.h</header>
+ <container>1</container>
+ </customwidget>
+ <customwidget>
+ <class>ControllerStatusBanner</class>
+ <extends>QWidget</extends>
+ <header>controllerstatusbanner.h</header>
+ <container>1</container>
+ </customwidget>
</customwidgets>
<resources/>
<connections/>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
new file mode 100644
index 0000000000000000000000000000000000000000..b9db68b0581266a44ef6644d15d5829746c9939d
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
@@ -0,0 +1,1285 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>CsoneControllerTab</class>
+ <widget class="QWidget" name="CsoneControllerTab">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>3197</width>
+ <height>1336</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="6" column="0">
+ <widget class="Line" name="line_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <layout class="QGridLayout" name="gridLayout_7">
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <item row="2" column="5">
+ <widget class="QLineEdit" name="lineEdit_overshoot_percent">
+ <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>---</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="9">
+ <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>
+ <item row="0" column="2">
+ <spacer name="horizontalSpacer_5">
+ <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="2" column="0">
+ <widget class="QLabel" name="label_step_end">
+ <property name="text">
+ <string>Step End</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="6">
+ <spacer name="horizontalSpacer_6">
+ <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="0">
+ <widget class="QLabel" name="label_step_start">
+ <property name="text">
+ <string>Step Start</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="5">
+ <widget class="QLineEdit" name="lineEdit_overshoot_value">
+ <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>---</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="1">
+ <widget class="QLineEdit" name="lineEdit_step_start">
+ <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>---</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="QLineEdit" name="lineEdit_step_end">
+ <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>---</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_overshoot_value">
+ <property name="text">
+ <string>Overshoot [meters]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="3">
+ <widget class="QLabel" name="label_overshoot_percent">
+ <property name="text">
+ <string>Overshoot [percent]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="7">
+ <widget class="QLabel" name="label_rise_time">
+ <property name="text">
+ <string>Rise Time</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="7">
+ <widget class="QLabel" name="label_rise_time_2">
+ <property name="text">
+ <string>Settling Time</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="8">
+ <widget class="QLineEdit" name="lineEdit_rise_time">
+ <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>---</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="8">
+ <widget class="QLineEdit" name="lineEdit_settling_time">
+ <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>---</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">
+ <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="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_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_time_series_time">
+ <property name="text">
+ <string>Time series for x position in meters</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="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="4" column="0">
+ <widget class="QChartView" name="chartView_for_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Maximum">
+ <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 row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout">
+ <property name="topMargin">
+ <number>50</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item row="0" column="8">
+ <widget class="QLabel" name="label_simulation">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Simulation</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_time_delay">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Time Delay</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="10">
+ <layout class="QGridLayout" name="gridLayout_8">
+ <item row="2" column="0">
+ <widget class="QPushButton" name="reset_integrator_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Reset</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_integrator_state">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>off</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QPushButton" name="toggle_integrator_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Toggle</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="9">
+ <spacer name="horizontalSpacer_7">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>100</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="5">
+ <spacer name="horizontalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>100</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_lead_compenstor">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Lead Compensator</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="12">
+ <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="1" column="0">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <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>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="lineEdit_alpha">
+ <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>0.10</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_T">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>T</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_alpha">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>alpha</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="lineEdit_k">
+ <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="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0.0160</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="lineEdit_T">
+ <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>4.0</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_k">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>k</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="QPushButton" name="set_lead_compensator_parameters_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="3">
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="1" column="0">
+ <widget class="QPushButton" name="set_pd_controller_parameters_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_5">
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_kd">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>k_d</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_kp">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>k_p</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="lineEdit_kd">
+ <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="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0.2</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="lineEdit_kp">
+ <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="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0.5</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="6">
+ <widget class="QLabel" name="label_step_details">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Step Details</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="6">
+ <layout class="QGridLayout" name="gridLayout_4">
+ <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_step_size">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Size</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_step_duration">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Duraion</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="lineEdit_step_size">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>300</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0.5</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="lineEdit_step_duration">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>300</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>10</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QPushButton" name="perform_step_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Perform</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QPushButton" name="log_data_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Log</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="1">
+ <spacer name="horizontalSpacer_pd_controller">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>100</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLabel" name="label_pid_controller">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>PD Controller</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="10">
+ <widget class="QLabel" name="label_integrator">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Integrator</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="11">
+ <spacer name="horizontalSpacer_8">
+ <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="1" column="2">
+ <layout class="QGridLayout" name="gridLayout_13">
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_time_delay_units">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>[milli sec]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QPushButton" name="set_time_delay_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="lineEdit_time_delay">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>300</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>0</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="7">
+ <spacer name="horizontalSpacer_simulation">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>100</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="1" column="8">
+ <layout class="QGridLayout" name="gridLayout_10">
+ <item row="2" column="0">
+ <widget class="QPushButton" name="clear_simulation_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Clear</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QPushButton" name="simulate_step_response_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>1000</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Perform</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_simulation_blank">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ <item row="7" column="0">
+ <spacer name="verticalSpacer_3">
+ <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>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="9" column="0">
+ <widget class="QChartView" name="chartView_for_pitch"/>
+ </item>
+ <item row="8" column="0">
+ <widget class="QLabel" name="label_time_series_time_2">
+ <property name="text">
+ <string>Time series for pitch angle in degrees</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <customwidgets>
+ <customwidget>
+ <class>QChartView</class>
+ <extends>QGraphicsView</extends>
+ <header>QtCharts</header>
+ </customwidget>
+ </customwidgets>
+ <tabstops>
+ <tabstop>lineEdit_k</tabstop>
+ <tabstop>lineEdit_T</tabstop>
+ <tabstop>lineEdit_alpha</tabstop>
+ <tabstop>lineEdit_time_delay</tabstop>
+ <tabstop>lineEdit_kp</tabstop>
+ <tabstop>lineEdit_kd</tabstop>
+ <tabstop>lineEdit_step_size</tabstop>
+ <tabstop>lineEdit_step_duration</tabstop>
+ <tabstop>set_lead_compensator_parameters_button</tabstop>
+ <tabstop>set_time_delay_button</tabstop>
+ <tabstop>set_pd_controller_parameters_button</tabstop>
+ <tabstop>perform_step_button</tabstop>
+ <tabstop>log_data_button</tabstop>
+ <tabstop>simulate_step_response_button</tabstop>
+ <tabstop>clear_simulation_button</tabstop>
+ <tabstop>toggle_integrator_button</tabstop>
+ <tabstop>reset_integrator_button</tabstop>
+ <tabstop>lineEdit_rise_time</tabstop>
+ <tabstop>lineEdit_settling_time</tabstop>
+ <tabstop>lineEdit_step_end</tabstop>
+ <tabstop>lineEdit_step_start</tabstop>
+ <tabstop>chartView_for_x</tabstop>
+ <tabstop>chartView_for_pitch</tabstop>
+ <tabstop>lineEdit_overshoot_value</tabstop>
+ <tabstop>lineEdit_overshoot_percent</tabstop>
+ </tabstops>
+ <resources/>
+ <connections/>
+</ui>
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
index c8225eaff2d6b6b9e9c3fc2b5d71237bce7b629c..496157dfaff3357a399e526701f7970672ff08c7 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/defaultcontrollertab.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/defaultcontrollertab.ui
@@ -320,6 +320,9 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>0.1</string>
+ </property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
@@ -386,6 +389,9 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>0.1</string>
+ </property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
@@ -512,6 +518,9 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>15</string>
+ </property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
@@ -826,6 +835,9 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>0.1</string>
+ </property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
index 7bafd6f49857ce5a3ebe291dbe62805069eaf7f8..d92fc08b2c7ba41f048c17b9d4c7bcdee4802863 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
@@ -30,8 +30,8 @@
<property name="spacing">
<number>12</number>
</property>
- <item row="1" column="1">
- <widget class="QPushButton" name="load_yaml_default_button">
+ <item row="1" column="5">
+ <widget class="QPushButton" name="load_yaml_tuning_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -51,12 +51,12 @@
</size>
</property>
<property name="text">
- <string>Default</string>
+ <string>Tuning</string>
</property>
</widget>
</item>
- <item row="1" column="3">
- <widget class="QPushButton" name="load_yaml_picker_button">
+ <item row="1" column="7">
+ <widget class="QPushButton" name="load_yaml_template_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -76,71 +76,43 @@
</size>
</property>
<property name="text">
- <string>Picker</string>
+ <string>Template</string>
</property>
</widget>
</item>
- <item row="0" column="0">
- <widget class="QLabel" name="enable_controller_label">
+ <item row="0" column="2">
+ <widget class="QPushButton" name="enable_student_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>0</width>
+ <width>60</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
- <width>16777215</width>
+ <width>180</width>
<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>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QPushButton" name="load_yaml_tuning_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Tuning</string>
+ <string>Student</string>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="load_yaml_label">
+ <item row="0" column="0">
+ <widget class="QLabel" name="enable_controller_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -166,7 +138,7 @@
</font>
</property>
<property name="text">
- <string>Load YAML</string>
+ <string>Enable</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@@ -198,8 +170,8 @@
</property>
</widget>
</item>
- <item row="1" column="6">
- <widget class="QPushButton" name="load_yaml_template_button">
+ <item row="0" column="5">
+ <widget class="QPushButton" name="enable_tuning_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -219,12 +191,12 @@
</size>
</property>
<property name="text">
- <string>Template</string>
+ <string>Tuning</string>
</property>
</widget>
</item>
- <item row="0" column="2">
- <widget class="QPushButton" name="enable_student_button">
+ <item row="0" column="1">
+ <widget class="QPushButton" name="enable_default_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -250,12 +222,12 @@
</font>
</property>
<property name="text">
- <string>Student</string>
+ <string>Default</string>
</property>
</widget>
</item>
<item row="0" column="4">
- <widget class="QPushButton" name="enable_tuning_button">
+ <widget class="QPushButton" name="enable_picker_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -274,13 +246,19 @@
<height>50</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
<property name="text">
- <string>Tuning</string>
+ <string>Picker</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QPushButton" name="enable_default_button">
+ <item row="0" column="7">
+ <widget class="QPushButton" name="enable_template_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -306,43 +284,46 @@
</font>
</property>
<property name="text">
- <string>Default</string>
+ <string>Template</string>
</property>
</widget>
</item>
- <item row="0" column="3">
- <widget class="QPushButton" name="enable_picker_button">
+ <item row="1" column="0">
+ <widget class="QLabel" name="load_yaml_label">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>60</width>
+ <width>0</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
- <width>180</width>
+ <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>Picker</string>
+ <string>Load YAML</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="0" column="6">
- <widget class="QPushButton" name="enable_template_button">
+ <item row="1" column="1">
+ <widget class="QPushButton" name="load_yaml_default_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -361,18 +342,12 @@
<height>50</height>
</size>
</property>
- <property name="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
<property name="text">
- <string>Template</string>
+ <string>Default</string>
</property>
</widget>
</item>
- <item row="0" column="7">
+ <item row="0" column="8">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -385,7 +360,7 @@
</property>
</spacer>
</item>
- <item row="0" column="5">
+ <item row="0" column="6">
<widget class="QPushButton" name="enable_remote_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
@@ -410,7 +385,7 @@
</property>
</widget>
</item>
- <item row="1" column="5">
+ <item row="1" column="6">
<widget class="QPushButton" name="load_yaml_remote_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
@@ -435,6 +410,81 @@
</property>
</widget>
</item>
+ <item row="1" column="4">
+ <widget class="QPushButton" name="load_yaml_picker_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Picker</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QPushButton" name="enable_csone_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>49</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>CS1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QPushButton" name="load_yaml_csone_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>CS1</string>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
</layout>
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
index e6f37426b294f7ef5deda40f757c4c081adcf752..aa37362f82fe3ccd8e170ddccc5d29abb8e9f4e9 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
@@ -190,7 +190,7 @@
<x>0</x>
<y>0</y>
<width>1862</width>
- <height>40</height>
+ <height>26</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -212,6 +212,7 @@
</property>
<addaction name="action_showHideController_default"/>
<addaction name="action_showHideController_student"/>
+ <addaction name="action_showHideController_csone"/>
<addaction name="action_showHideController_picker"/>
<addaction name="action_showHideController_tuning"/>
<addaction name="action_showHideController_remote"/>
@@ -332,6 +333,17 @@
<string>Remote</string>
</property>
</action>
+ <action name="action_showHideController_csone">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>CS1</string>
+ </property>
+ </action>
</widget>
<layoutdefault spacing="6" margin="11"/>
<customwidgets>
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
index e3e70de43498c3add8cd56bf6c419f1759bf1a69..c100ec79808254f76fdba5cbc5e6a22216fd8e2f 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/studentcontrollertab.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/studentcontrollertab.ui
@@ -456,6 +456,9 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>0.1</string>
+ </property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
@@ -1028,6 +1031,9 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>0.1</string>
+ </property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
@@ -1094,6 +1100,9 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>15</string>
+ </property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
@@ -1160,6 +1169,9 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>0.1</string>
+ </property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
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
index 723bdf8da175f7ea13ff3db977ae2cbc79d87b62..3a40cb92a0ad3d03ee4a9e8d3fc49615425e5817 100644
--- 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
@@ -115,6 +115,7 @@
#define TUNING_CONTROLLER 6
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
+#define CSONE_CONTROLLER 9
#define TESTMOTORS_CONTROLLER 21
@@ -128,6 +129,7 @@
#define CMD_USE_TUNING_CONTROLLER 6
#define CMD_USE_PICKER_CONTROLLER 7
#define CMD_USE_TEMPLATE_CONTROLLER 8
+#define CMD_USE_CSONE_CONTROLLER 9
#define CMD_CRAZYFLY_TAKE_OFF 11
@@ -203,6 +205,25 @@
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// These constants deine the current and interaction with an intergrator
+#define INTEGRATOR_FLAG_ON 1
+#define INTEGRATOR_FLAG_OFF 2
+#define INTEGRATOR_FLAG_UNKNOWN 3
+#define INTEGRATOR_FLAG_TOGGLE 4
+#define INTEGRATOR_FLAG_RESET 5
+
+
+
+
+
// ----------------------------------------------------------------------------------
//
//
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllerstatusbanner.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllerstatusbanner.h
new file mode 100644
index 0000000000000000000000000000000000000000..7a524ea7c1d9f733951ac1d3a44986f00ca76290
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllerstatusbanner.h
@@ -0,0 +1,87 @@
+// Copyright (C) 2020, 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 banner that indicates the status of a controller.
+// Possible status values are: { OFF , ACTIVE , COORD }
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef CONTROLLERSTATUSBANNER_H
+#define CONTROLLERSTATUSBANNER_H
+
+#include <QWidget>
+#include <QMutex>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+#else
+#include <QTextStream>
+#endif
+
+// STATUS VALUES
+#define CONTROLLER_STATUS_BANNER_OFF 0
+#define CONTROLLER_STATUS_BANNER_ACTIVE 1
+#define CONTROLLER_STATUS_BANNER_COORD 2
+
+
+namespace Ui {
+class ControllerStatusBanner;
+}
+
+class ControllerStatusBanner : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit ControllerStatusBanner(QWidget *parent = 0);
+ ~ControllerStatusBanner();
+
+ // PUBLIC METHODS FOR SETTING PROPERTIES
+ // > Set the state of the checkbox
+ void setStatus(int new_status);
+
+private:
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+ Ui::ControllerStatusBanner *ui;
+
+ // > For the current status
+ int m_current_status;
+
+ // MUTEX FOR HANDLING ACCESS
+ // > For the status variable and UI elements
+ QMutex m_status_mutex;
+
+};
+
+#endif // CONTROLLERSTATUSBANNER_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
index 74f0e5b0c1882a679a3f0691764e062e18de398c..08df7f2abf692151d88cdb129af0466fc6f39d29 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
@@ -37,9 +37,12 @@
#ifndef CONTROLLERTABS_H
#define CONTROLLERTABS_H
+#include "controllerstatusbanner.h"
+
#include <QWidget>
#include <QMutex>
#include <QVector>
+//#include <QSoundEffect>
#ifdef CATKIN_MAKE
#include <ros/ros.h>
@@ -95,6 +98,7 @@ public:
void showHideController_tuning_changed();
void showHideController_remote_changed();
void showHideController_template_changed();
+ void showHideController_csone_changed();
public slots:
@@ -136,9 +140,11 @@ private:
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;
+ // Mutex for the highlighting of the active controller
+ QMutex m_change_highlighted_controller_mutex;
+
+ // Sound effect for when the controller changes while flying
+ //QSoundEffect m_soundEffect_controllerChanged;
#ifdef CATKIN_MAKE
@@ -178,9 +184,15 @@ private:
void setControllerEnabled(int new_controller);
+ void setTextColourOfTabLabel(QColor color , QWidget * tab_widget);
+
void setAllTabLabelsToNormalColouring();
- void setTextColourOfTabLabel(QColor color , QWidget * tab_widget);
+ void setAllBannersToControllerIsOff();
+
+ void setAllBannersToControllerCoordinator();
+
+
};
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
new file mode 100644
index 0000000000000000000000000000000000000000..5ba4e0551fd52aba071661895065dfd3b6e7d102
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
@@ -0,0 +1,295 @@
+// 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 Csone Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+// NOTE: Instructions for how to add a Chart are taken from this stack overflow answer:
+// https://stackoverflow.com/questions/48362864/how-to-insert-qchartview-in-form-with-qt-designer
+//
+// Option 1: Promoted
+// 1) first add QT += charts in the .pro
+// 2) place the QGraphicsView to the design.
+// 3) Right click on the QGraphicsView and select Promote to...
+// 4) When doing the above, a menu appears, in the menu it should be set in QChartView in Promoted
+// Class Name, and QtCharts in Header file, then press the add button and finally press promote.
+//
+// Option 2: QtChart plugin
+// This option was NOT used, see answer on stack overflow for further details
+
+
+
+
+#ifndef CSONECONTROLLERTAB_H
+#define CSONECONTROLLERTAB_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QVector>
+#include <QLineEdit>
+#include <QTextStream>
+
+// For the chart:
+#include <QChart>
+#include <QLineSeries>
+#include <QList>
+#include <QPointF>
+using namespace QtCharts;
+
+#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/IntIntService.h"
+#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 CsoneControllerTab;
+}
+
+class CsoneControllerTab : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit CsoneControllerTab(QWidget *parent = 0);
+ ~CsoneControllerTab();
+
+
+
+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:
+
+ float validate_and_get_value_from_lineEdit(QLineEdit * lineEdit, float min, float max, int decimals, float default_value);
+
+ void newDataForPerformingStepAndPlotting(float x, float pitch);
+ void analyseStepResponse();
+ void updateStepAnalysisLineEdit(float value, QLineEdit * lineEdit, int num_dec_places);
+
+
+ void on_perform_step_button_clicked();
+ void on_log_data_button_clicked();
+
+ void on_simulate_step_response_button_clicked();
+ void on_clear_simulation_button_clicked();
+
+ void on_set_lead_compensator_parameters_button_clicked();
+
+ void on_set_time_delay_button_clicked();
+
+ void on_toggle_integrator_button_clicked();
+ void on_reset_integrator_button_clicked();
+
+ void on_lineEdit_k_returnPressed();
+ void on_lineEdit_T_returnPressed();
+ void on_lineEdit_alpha_returnPressed();
+ void on_lineEdit_kp_returnPressed();
+ void on_lineEdit_kd_returnPressed();
+ void on_lineEdit_time_delay_returnPressed();
+ void on_lineEdit_step_size_returnPressed();
+ void on_lineEdit_step_duration_returnPressed();
+
+ void on_lineEdit_k_editingFinished();
+ void on_lineEdit_T_editingFinished();
+ void on_lineEdit_alpha_editingFinished();
+
+ void on_lineEdit_step_size_editingFinished();
+ void on_lineEdit_step_duration_editingFinished();
+
+
+
+ void simulate_step_response();
+
+
+private:
+ Ui::CsoneControllerTab *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;
+
+ // For the current setpoints
+ float m_current_setpoint_x = 0.0;
+ float m_current_setpoint_y = 0.0;
+ float m_current_setpoint_z = 0.4;
+ float m_current_setpoint_yaw = 0.0;
+
+ // > Mutex for serialising acess to the controller variables
+ QMutex m_controller_parameter_mutex;
+
+ // FOR PLOTTING DATA ON THE CHART
+ // > Mutex for serialising acess to any charting variable
+ QMutex m_chart_mutex;
+ // > Flag for whether to perform a step
+ bool m_shouldPerformStep = false;
+ // > Flag for whether to store data for plotting
+ bool m_shouldStoreData_for_plotting = false;
+ // > Flag for reseting the zoom level
+ bool m_shouldResetZoom = false;
+ // > Time (as a float) for the horizontal axis
+ float m_time_for_step = 0.0f;
+ // > The duration for which to record the step
+ float m_step_response_data_recording_duration = 10.0f;
+ // > Line Series for the x position data
+ QLineSeries *m_lineSeries_for_setpoint_x;
+ QLineSeries *m_lineSeries_for_measured_x;
+ // > Line Series for the pitch angle data
+ //QLineSeries *m_lineSeries_for_setpoint_pitch;
+ QLineSeries *m_lineSeries_for_measured_pitch;
+
+
+ // FOR SIMULATING THE CONTROLLER
+ // > Mutex for serialising acess to any simulation variables
+ QMutex m_simulation_mutex;
+
+ // Flag for whether a simulation is under way
+ bool m_simulationIsInProgress = false;
+
+ // The state space matrices of lead compensator controller
+ float m_lead_compensator_A=0.987578;
+ float m_lead_compensator_B=0.00496888;
+ float m_lead_compensator_C=-0.36;
+ float m_lead_compensator_D=0.16;
+
+ // The state of lead compensator controller
+ float m_lead_compensator_state = 0.0;
+
+ // Line Series for plotting the simulation results
+ QLineSeries *m_lineSeries_for_sim_setpoint_x;
+ QLineSeries *m_lineSeries_for_sim_measured_x;
+ QLineSeries *m_lineSeries_for_sim_measured_pitch;
+
+
+
+#ifdef CATKIN_MAKE
+ // PUBLISHER
+ // > For requesting the setpoint to be changed
+ ros::Publisher requestSetpointChangePublisher;
+
+ // > For requesting the controller paramters to be changed
+ ros::Publisher requestControllerParamterChangePublisher;
+
+ // > For requesting a change in the integrator state
+ ros::Publisher requestIntegratorStateChangePublisher;
+
+ // > For requesting the time delay to be changed
+ ros::Publisher requestTimeDelayChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
+
+ // > For being notified when the integrator state
+ ros::Subscriber integratorStateChangedSubscriber;
+
+ // 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);
+
+ // For receiving message that the integrator state changes
+ void integratorStateChangedCallback(const dfall_pkg::IntWithHeader& newIntegratorState);
+
+ // Fill the header for a message
+ void fillIntMessageHeader( dfall_pkg::IntWithHeader & msg );
+ 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 publishControllerParamters(float k, float T, float alpha);
+
+ void publishRequestForIntegratorStateChange(int flag_to_publish);
+
+ void publishRequestForTimeDelayChange(int time_delay_to_publish);
+
+ void convertIntoDiscreteTimeParameters(float k, float T, float alpha);
+
+};
+
+#endif // CSONECONTROLLERTAB_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
index c48d711f9b009dd51da036b2dae4c56b41890c9b..c411e2d12fc07b1844615f6ed68c843a239e9aa0 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
@@ -94,7 +94,7 @@ public:
void showHideController_tuning_changed();
void showHideController_remote_changed();
void showHideController_template_changed();
-
+ void showHideController_csone_changed();
void testMotors_triggered();
@@ -111,6 +111,7 @@ private slots:
void on_enable_tuning_button_clicked();
void on_enable_remote_button_clicked();
void on_enable_template_button_clicked();
+ void on_enable_csone_button_clicked();
// LOAD YAML BUTTONS ON-CLICK CALLBACK
void on_load_yaml_default_button_clicked();
@@ -119,6 +120,7 @@ private slots:
void on_load_yaml_tuning_button_clicked();
void on_load_yaml_remote_button_clicked();
void on_load_yaml_template_button_clicked();
+ void on_load_yaml_csone_button_clicked();
private:
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
index ed4c43b6c4a052caa3768cf7b46d610c6953370a..09cab55768363e2a94de5314c39ba75f9642b009 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
@@ -133,6 +133,7 @@ private slots:
void on_action_showHideController_tuning_changed();
void on_action_showHideController_remote_changed();
void on_action_showHideController_template_changed();
+ void on_action_showHideController_csone_changed();
void on_action_testMotors_triggered();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h
index 686b250bf9369825a0fff7141f0475c5c461cbc7..60d50bc203614b14d18b457e91d3acabe21704dc 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h
@@ -109,11 +109,6 @@ private slots:
void on_default_setpoint_button_clicked();
- void on_custom_button_1_clicked();
- void on_custom_button_2_clicked();
- void on_custom_button_3_clicked();
-
-
// COPIED FROM PREVIOUS REMOTE CONTROLLER GUI
void on_subscribe_button_clicked();
void on_unsubscribe_button_clicked();
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
index c82fc96bf01161a5c59df58bf1edf0c5a8d7770b..fda9d841d2be9772dc10f89f5290090ec61ad480 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp
@@ -106,7 +106,7 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
// SUBSCRIBERS AND PUBLISHERS:
// > For Crazyradio commands based on button clicks
- crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/crazyRadioCommand", 1);
+ 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);
@@ -126,7 +126,7 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
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);
+ flyingStateSubscriber = base_nodeHandle.subscribe("FlyingAgentClient/FlyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
}
#endif
@@ -614,10 +614,10 @@ void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
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);
+ 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));
+ getCurrentFlyingStateService.waitForExistence(ros::Duration(0.0));
if(getCurrentFlyingStateService.call(getFlyingStateCall))
{
setFlyingState(getFlyingStateCall.response.data);
@@ -631,7 +631,7 @@ void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
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));
+ getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(0.0));
if(getCurrentCrazyRadioStateService.call(getCrazyRadioCall))
{
setCrazyRadioStatus(getCrazyRadioCall.response.data);
@@ -654,7 +654,7 @@ void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
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);
+ flyingStateSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/FlyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
}
else
{
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllerstatusbanner.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllerstatusbanner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5063c21cdda79f36ff0b457231181ac4045403c5
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllerstatusbanner.cpp
@@ -0,0 +1,79 @@
+#include "controllerstatusbanner.h"
+#include "ui_controllerstatusbanner.h"
+
+ControllerStatusBanner::ControllerStatusBanner(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::ControllerStatusBanner)
+{
+ ui->setupUi(this);
+
+ // HIDE ALL EXCEPT THE "OFF" BANNER
+ ui->frame_isOff->show();
+ ui->frame_isActive->hide();
+ ui->frame_isCoord->hide();
+}
+
+ControllerStatusBanner::~ControllerStatusBanner()
+{
+ delete ui;
+}
+
+
+
+
+
+// PUBLIC METHODS FOR SETTING PROPERTIES
+
+// > For making the "enable flight" and "disable flight" buttons unavailable
+void ControllerStatusBanner::setStatus(int new_status)
+{
+ // LOCK THE MUTEX
+ m_status_mutex.lock();
+
+ // CHECK IF THE "new_status" IS DIFFERENT FROM THE "m_current_status"
+ if (new_status!=m_current_status)
+ {
+ // UPDATE THE CLASS VARIABLE TO THE NEW STATUS
+ m_current_status = new_status;
+
+ // SWITCH BETWEEN THE POSSIBLE STATUS VALUES
+ switch (new_status)
+ {
+ case CONTROLLER_STATUS_BANNER_OFF:
+ {
+ ui->frame_isOff->show();
+ ui->frame_isActive->hide();
+ ui->frame_isCoord->hide();
+ break;
+ }
+ case CONTROLLER_STATUS_BANNER_ACTIVE:
+ {
+ ui->frame_isOff->hide();
+ ui->frame_isActive->show();
+ ui->frame_isCoord->hide();
+ break;
+ }
+ case CONTROLLER_STATUS_BANNER_COORD:
+ {
+ ui->frame_isOff->hide();
+ ui->frame_isActive->hide();
+ ui->frame_isCoord->show();
+ break;
+ }
+ default:
+ {
+ // INFORM THE USER OF THE ERROR
+ #ifdef CATKIN_MAKE
+ ROS_ERROR("[CONTROLLER STATUS BANNER] the new_status requested is not a valid status value.");
+ #else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CONTROLLER STATUS BANNER] the new_status requested is not a valid status value.";
+ #endif
+ break;
+ }
+ }
+ }
+
+ // UNLOCK THE MUTEX
+ m_status_mutex.unlock();
+}
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
index ac62ee9e0272cce08d27414640f317be7999aa90..c29d10d692ea6cf2e3e89f0bf6af1558c509b5b4 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
@@ -44,9 +44,15 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
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);
+ // Set all the banners to be the inactive colouring
+ setAllBannersToControllerIsOff();
+
+ // Initialise the sound effect for when the controller changes while flying
+ //QSoundEffect m_soundEffect_controllerChanged;
+ //m_soundEffect_controllerChanged.setSource(QUrl::fromLocalFile("engine.wav"));
+ //m_soundEffect_controllerChanged.setLoopCount(1);
+ //m_soundEffect_controllerChanged.setVolume(0.25f);
+ //m_soundEffect_controllerChanged.play();
// Initialise the object name as blank
@@ -87,6 +93,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->template_controller_tab_widget , &TemplateControllerTab::setMeasuredPose
);
+ QObject::connect(
+ this , &ControllerTabs::measuredPoseValueChanged ,
+ ui->csone_controller_tab_widget , &CsoneControllerTab::setMeasuredPose
+ );
+
// CONNECT THE "MEASUREMENTS UNAVAILABLE" SIGNAL TO
@@ -121,6 +132,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->template_controller_tab_widget , &TemplateControllerTab::poseDataUnavailableSlot
);
+ QObject::connect(
+ this , &ControllerTabs::poseDataUnavailableSignal ,
+ ui->csone_controller_tab_widget , &CsoneControllerTab::poseDataUnavailableSlot
+ );
+
// CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
// WITH THE LIST OF AGENT IDs TO COORDINATE
@@ -157,6 +173,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->template_controller_tab_widget , &TemplateControllerTab::setAgentIDsToCoordinate
);
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->csone_controller_tab_widget , &CsoneControllerTab::setAgentIDsToCoordinate
+ );
+
@@ -194,7 +215,7 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
// Only if this is an agent GUI
if (m_type == TYPE_AGENT)
{
- controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
+ controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
}
@@ -203,7 +224,7 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ros::NodeHandle dfall_root_nodeHandle("/dfall");
// > Publisher for the emergency stop button
- //emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("emergencyStop", 1);
+ //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);;
@@ -272,6 +293,11 @@ void ControllerTabs::showHideController_template_changed()
showHideController_toggle("Template",ui->template_tab);
}
+void ControllerTabs::showHideController_csone_changed()
+{
+ showHideController_toggle("CS1",ui->csone_tab);
+}
+
@@ -402,7 +428,7 @@ void ControllerTabs::poseDataReceivedCallback(const dfall_pkg::ViconData& viconD
// > 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);
+ //ROS_INFO_STREAM("[COORDINATOR ROW GUI] Controller Used Changed Callback called with msg.data = " << msg.data);
setControllerEnabled(msg.data);
}
#endif
@@ -410,8 +436,15 @@ void ControllerTabs::controllerUsedChangedCallback(const std_msgs::Int32& msg)
void ControllerTabs::setControllerEnabled(int new_controller)
{
+ // Define the tab text highlight colour locally
+ const static QColor tab_text_colour_highlight = QColor(40,120,40);
+
// First set everything back to normal colouring
setAllTabLabelsToNormalColouring();
+ setAllBannersToControllerIsOff();
+
+ // Lock the mutex
+ m_change_highlighted_controller_mutex.lock();
// Now switch to highlight the tab corresponding to
// the enable controller
@@ -419,7 +452,8 @@ void ControllerTabs::setControllerEnabled(int new_controller)
{
case DEFAULT_CONTROLLER:
{
- setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->default_tab );
+ setTextColourOfTabLabel( tab_text_colour_highlight , ui->default_tab );
+ ui->statusBanner_default->setStatus(CONTROLLER_STATUS_BANNER_ACTIVE);
break;
}
case DEMO_CONTROLLER:
@@ -429,58 +463,62 @@ void ControllerTabs::setControllerEnabled(int new_controller)
}
case STUDENT_CONTROLLER:
{
- setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->student_tab );
+ setTextColourOfTabLabel( tab_text_colour_highlight , ui->student_tab );
+ ui->statusBanner_student->setStatus(CONTROLLER_STATUS_BANNER_ACTIVE);
break;
}
case MPC_CONTROLLER:
{
- //ui->controller_enabled_label->setText("MPC");
+ //...
break;
}
case REMOTE_CONTROLLER:
{
- setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->remote_tab );
+ setTextColourOfTabLabel( tab_text_colour_highlight , ui->remote_tab );
+ ui->statusBanner_remote->setStatus(CONTROLLER_STATUS_BANNER_ACTIVE);
break;
}
case TUNING_CONTROLLER:
{
- setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->tuning_tab );
+ setTextColourOfTabLabel( tab_text_colour_highlight , ui->tuning_tab );
+ ui->statusBanner_tuning->setStatus(CONTROLLER_STATUS_BANNER_ACTIVE);
break;
}
case PICKER_CONTROLLER:
{
- setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->picker_tab );
+ setTextColourOfTabLabel( tab_text_colour_highlight , ui->picker_tab );
+ ui->statusBanner_picker->setStatus(CONTROLLER_STATUS_BANNER_ACTIVE);
break;
}
case TEMPLATE_CONTROLLER:
{
- setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->template_tab );
+ setTextColourOfTabLabel( tab_text_colour_highlight , ui->template_tab );
+ ui->statusBanner_template->setStatus(CONTROLLER_STATUS_BANNER_ACTIVE);
+ break;
+ }
+ case CSONE_CONTROLLER:
+ {
+ setTextColourOfTabLabel( tab_text_colour_highlight , ui->csone_tab );
+ ui->statusBanner_csone->setStatus(CONTROLLER_STATUS_BANNER_ACTIVE);
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->remote_tab );
- setTextColourOfTabLabel( m_tab_text_colour_normal , ui->template_tab );
+ // Unlock the mutex
+ m_change_highlighted_controller_mutex.unlock();
}
+
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
+ // Only apply the colour if the tab is found
if (current_index_of_tab >= 0)
{
ui->controller_tabs_widget->tabBar()->setTabTextColor(current_index_of_tab, color);
@@ -488,6 +526,66 @@ void ControllerTabs::setTextColourOfTabLabel(QColor color , QWidget * tab_widget
}
+void ControllerTabs::setAllTabLabelsToNormalColouring()
+{
+ // Define the tab text normal colour locally
+ const static QColor tab_text_colour_normal = Qt::black;
+
+ // Lock the mutex
+ m_change_highlighted_controller_mutex.lock();
+
+ // Set all the colours to normal
+ setTextColourOfTabLabel( tab_text_colour_normal , ui->default_tab );
+ setTextColourOfTabLabel( tab_text_colour_normal , ui->student_tab );
+ setTextColourOfTabLabel( tab_text_colour_normal , ui->picker_tab );
+ setTextColourOfTabLabel( tab_text_colour_normal , ui->tuning_tab );
+ setTextColourOfTabLabel( tab_text_colour_normal , ui->remote_tab );
+ setTextColourOfTabLabel( tab_text_colour_normal , ui->template_tab );
+ setTextColourOfTabLabel( tab_text_colour_normal , ui->csone_tab );
+
+ // Unlock the mutex
+ m_change_highlighted_controller_mutex.unlock();
+}
+
+void ControllerTabs::setAllBannersToControllerIsOff()
+{
+ // Lock the mutex
+ m_change_highlighted_controller_mutex.lock();
+
+ // Set all the banners to the off status
+ ui->statusBanner_default ->setStatus(CONTROLLER_STATUS_BANNER_OFF);
+ ui->statusBanner_student ->setStatus(CONTROLLER_STATUS_BANNER_OFF);
+ ui->statusBanner_picker ->setStatus(CONTROLLER_STATUS_BANNER_OFF);
+ ui->statusBanner_tuning ->setStatus(CONTROLLER_STATUS_BANNER_OFF);
+ ui->statusBanner_remote ->setStatus(CONTROLLER_STATUS_BANNER_OFF);
+ ui->statusBanner_template->setStatus(CONTROLLER_STATUS_BANNER_OFF);
+ ui->statusBanner_csone ->setStatus(CONTROLLER_STATUS_BANNER_OFF);
+
+ // Unlock the mutex
+ m_change_highlighted_controller_mutex.unlock();
+}
+
+void ControllerTabs::setAllBannersToControllerCoordinator()
+{
+ // Lock the mutex
+ m_change_highlighted_controller_mutex.lock();
+
+ // Set all the banners to the coordinator status
+ ui->statusBanner_default ->setStatus(CONTROLLER_STATUS_BANNER_COORD);
+ ui->statusBanner_student ->setStatus(CONTROLLER_STATUS_BANNER_COORD);
+ ui->statusBanner_picker ->setStatus(CONTROLLER_STATUS_BANNER_COORD);
+ ui->statusBanner_tuning ->setStatus(CONTROLLER_STATUS_BANNER_COORD);
+ ui->statusBanner_remote ->setStatus(CONTROLLER_STATUS_BANNER_COORD);
+ ui->statusBanner_template->setStatus(CONTROLLER_STATUS_BANNER_COORD);
+ ui->statusBanner_csone ->setStatus(CONTROLLER_STATUS_BANNER_COORD);
+
+ // Unlock the mutex
+ m_change_highlighted_controller_mutex.unlock();
+}
+
+
+
+
// ----------------------------------------------------------------------------------
// A GGGG EEEEE N N TTTTT III DDDD SSSS
@@ -534,10 +632,10 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should
// > Request the current instant controller
- ros::ServiceClient getInstantControllerService = agent_base_nodeHandle.serviceClient<dfall_pkg::IntIntService>("FlyingAgentClient/getInstantController", false);
+ 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));
+ getInstantControllerService.waitForExistence(ros::Duration(0.1));
if(getInstantControllerService.call(getInstantControllerCall))
{
setControllerEnabled(getInstantControllerCall.response.data);
@@ -549,16 +647,22 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should
// SUBSCRIBERS
// > For receiving message that the instant controller was changed
- controllerUsedSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/controllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
+ m_change_highlighted_controller_mutex.lock();
+ controllerUsedSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
+ m_change_highlighted_controller_mutex.unlock();
}
else
{
// Unsubscribe
+ m_change_highlighted_controller_mutex.lock();
controllerUsedSubscriber.shutdown();
+ m_change_highlighted_controller_mutex.unlock();
// Set all tabs to be normal colours
setAllTabLabelsToNormalColouring();
+ // Set all current controller banners to be in "coordinator mode"
+ setAllBannersToControllerCoordinator();
}
#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
index b6d60f69027d02249278daa7a449ac0c111beb90..eb55555bc6b40e703d2f6ec23c5d8113aea52005 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
@@ -109,7 +109,7 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
// SUBSCRIBERS AND PUBLISHERS:
// > For Crazyradio commands based on button clicks
- crazyRadioCommandPublisher = base_nodeHandle.advertise<dfall_pkg::IntWithHeader>("FlyingAgentClient/crazyRadioCommand", 1);
+ 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);
@@ -123,18 +123,18 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
// > 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);
+ 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);
+ 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);
+ 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
@@ -649,7 +649,7 @@ void CoordinatorRow::getCurrentFlyingState()
#ifdef CATKIN_MAKE
dfall_pkg::IntIntService getFlyingStateCall;
getFlyingStateCall.request.data = 0;
- getCurrentFlyingStateService.waitForExistence(ros::Duration(2.0));
+ getCurrentFlyingStateService.waitForExistence(ros::Duration(0.0));
if(getCurrentFlyingStateService.call(getFlyingStateCall))
{
setFlyingState(getFlyingStateCall.response.data);
@@ -669,7 +669,7 @@ void CoordinatorRow::getCurrentCrazyRadioState()
#ifdef CATKIN_MAKE
dfall_pkg::IntIntService getCrazyRadioCall;
getCrazyRadioCall.request.data = 0;
- getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(2.0));
+ getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(0.0));
if(getCurrentCrazyRadioStateService.call(getCrazyRadioCall))
{
setCrazyRadioStatus(getCrazyRadioCall.response.data);
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..401093fbe7b2f54dedc52a6e2c33f51e6375457e
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
@@ -0,0 +1,1835 @@
+// 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 Csone Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "csonecontrollertab.h"
+#include "ui_csonecontrollertab.h"
+
+CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::CsoneControllerTab)
+{
+ 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);
+
+ // ------------------------------------------------------------- //
+ // HIDE THE PD CONTROLLER FIELDS
+ ui->label_pid_controller->hide();
+ ui->label_kp->hide();
+ ui->label_kd->hide();
+ ui->lineEdit_kp->hide();
+ ui->lineEdit_kd->hide();
+ ui->set_pd_controller_parameters_button->hide();
+
+
+ // ------------------------------------------------------------- //
+ // HIDE THE SIMULATION BUTTONS
+ //ui->label_simulation->hide();
+ //ui->label_simulation_blank->hide();
+ //ui->simulate_step_response_button->hide();
+ //ui->clear_simulation_button->hide();
+
+
+ // ------------------------------------------------------------- //
+ // SET VALIDATORS FOR THE LINE EDITS
+ // > NOT USED BECAUSE DO NOT AUTOMATICALLY CLIP TO THE MIN AND MAX
+ //ui->lineEdit_k->setValidator(new QDoubleValidator(-1.0,1.0,2,ui->lineEdit_k) );
+
+
+ // ------------------------------------------------------------- //
+ // SETUP THE CHART VIEW FOR THE X POSITION
+ //
+ // Set the sizing policy for the chart
+ // > This needs to be set here because the sizing policy needs to be
+ // set for the chart, but only the sizing policy for the
+ // ui->chartView_for_x object is accessible through the graphical
+ // designer for the .ui file
+ // > Syntax hint:
+ // void QWidget::setSizePolicy(QSizePolicy::Policy horizontal, QSizePolicy::Policy vertical)
+ ui->chartView_for_x->setSizePolicy(QSizePolicy::MinimumExpanding,QSizePolicy::Expanding);
+ ui->chartView_for_x->chart()->setMinimumHeight(600);
+
+ // Hide the legend of the chart
+ ui->chartView_for_x->chart()->legend()->hide();
+
+ // Set the chart to have no title
+ ui->chartView_for_x->chart()->setTitle("");
+
+ // Set the theme of the chart
+ ui->chartView_for_x->chart()->setTheme(QChart::ChartThemeLight);
+
+ // Initialise the line series to be used for plotting
+ m_lineSeries_for_setpoint_x = new QLineSeries();
+ m_lineSeries_for_measured_x = new QLineSeries();
+
+ m_lineSeries_for_sim_setpoint_x = new QLineSeries();
+ m_lineSeries_for_sim_measured_x = new QLineSeries();
+
+ // Add the line series to the chart
+ ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_setpoint_x);
+ ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_measured_x);
+
+ ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_sim_setpoint_x);
+ ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_sim_measured_x);
+
+ // Set the style of the series
+ m_lineSeries_for_setpoint_x->setPen(QPen( QBrush("blue") , 5.0 ));
+ m_lineSeries_for_measured_x->setPen(QPen( QBrush("red") , 5.0 ));
+
+ m_lineSeries_for_sim_setpoint_x->setPen(QPen( QBrush("blue") , 5.0 , Qt::DotLine ));
+ m_lineSeries_for_sim_measured_x->setPen(QPen( QBrush("red") , 5.0 , Qt::DotLine ));
+
+ // OPTIONS FOR THE LINE STYLE:
+ // { Qt::SolidLine , Qt::DashLine , Qt::DotLine , Qt::DashDotLine , Qt::DashDotDotLine }
+
+ // Set the initial axes limits
+ ui->chartView_for_x->chart()->createDefaultAxes();
+ ui->chartView_for_x->chart()->axisX()->setMin(-1.0);
+ ui->chartView_for_x->chart()->axisX()->setMax(m_step_response_data_recording_duration);
+ ui->chartView_for_x->chart()->axisY()->setMin(-0.6);
+ ui->chartView_for_x->chart()->axisY()->setMax( 0.6);
+
+
+ // ------------------------------------------------------------- //
+ // SETUP THE CHART VIEW FOR THE PITCH ANGLE
+ //
+ // Set the sizing policy for the chart
+ ui->chartView_for_pitch->setSizePolicy(QSizePolicy::MinimumExpanding,QSizePolicy::Expanding);
+ ui->chartView_for_pitch->chart()->setMinimumHeight(600);
+
+ // Hide the legend of the chart
+ ui->chartView_for_pitch->chart()->legend()->hide();
+
+ // Set the chart to have no title
+ ui->chartView_for_pitch->chart()->setTitle("");
+
+ // Set the theme of the chart
+ ui->chartView_for_pitch->chart()->setTheme(QChart::ChartThemeLight);
+
+ // Initialise the line series to be used for plotting
+ //m_lineSeries_for_setpoint_pitch = new QLineSeries();
+ m_lineSeries_for_measured_pitch = new QLineSeries();
+ m_lineSeries_for_sim_measured_pitch = new QLineSeries();
+
+ // Add the line series to the chart
+ //ui->chartView_for_pitch->chart()->addSeries(m_lineSeries_for_setpoint_pitch);
+ ui->chartView_for_pitch->chart()->addSeries(m_lineSeries_for_measured_pitch);
+ ui->chartView_for_pitch->chart()->addSeries(m_lineSeries_for_sim_measured_pitch);
+
+ // Set the style of the series
+ //m_lineSeries_for_setpoint_pitch->setPen(QPen( QBrush("blue") , 5.0 ));
+ m_lineSeries_for_measured_pitch->setPen(QPen( QBrush("red") , 5.0 ));
+ m_lineSeries_for_sim_measured_pitch->setPen(QPen( QBrush("red") , 5.0 , Qt::DotLine ));
+
+ // Set the initial axes limits
+ ui->chartView_for_pitch->chart()->createDefaultAxes();
+ ui->chartView_for_pitch->chart()->axisX()->setMin(-1.0);
+ ui->chartView_for_pitch->chart()->axisX()->setMax(m_step_response_data_recording_duration);
+ ui->chartView_for_pitch->chart()->axisY()->setMin(-20.0);
+ ui->chartView_for_pitch->chart()->axisY()->setMax(20.0);
+
+
+
+
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CSONE 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("[CSONE 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>("CsoneControllerService/RequestSetpointChange", 1);
+
+ // CREATE THE REQUEST CONTROLLER PARAMTER CHANGE PUBLISHER
+ requestControllerParamterChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("CsoneControllerService/RequestControllerParametersChange", 1);
+
+ // CREATE THE REQUEST CONTROLLER PARAMTER CHANGE PUBLISHER
+ requestTimeDelayChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::IntWithHeader>("CsoneControllerService/RequestTimeDelayChange", 1);
+
+ // CREATE THE REQUEST INTEGRATOR STATE CHANGE PUBLISHER
+ requestIntegratorStateChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::IntWithHeader>("CsoneControllerService/RequestIntegratorStateChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES AND INTEGRATOR STATE CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("CsoneControllerService/SetpointChanged" , 1, &CsoneControllerTab::setpointChangedCallback , this);
+ integratorStateChangedSubscriber = nodeHandle_for_this_gui.subscribe("CsoneControllerService/IntegratorStateChanged", 1, &CsoneControllerTab::integratorStateChangedCallback, this);
+ }
+
+ // CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
+ customButtonPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::CustomButtonWithHeader>("CsoneControllerService/CustomButtonPressed", 1);
+
+ // GET THE CURRENT SETPOINT AND CURRENT INTEGRATOR STATE
+ // 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>("CsoneControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[CSONE CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+
+ // > Request the current integrator state
+ ros::ServiceClient getCurrentIntegatorStateServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::IntIntService>("CsoneControllerService/GetCurrentIntegratorState", false);
+ dfall_pkg::IntIntService getIntegrtorStateCall;
+ getIntegrtorStateCall.request.data = 0;
+ getCurrentIntegatorStateServiceClient.waitForExistence(ros::Duration(0.1));
+ if(getCurrentIntegatorStateServiceClient.call(getIntegrtorStateCall))
+ {
+ dfall_pkg::IntWithHeader temp_msg;
+ temp_msg.data = getIntegrtorStateCall.response.data;
+ integratorStateChangedCallback(temp_msg);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[CSONE CONTROLLER GUI] Failed to get current integrator state from controller using the \"GetCurrentIntegratorState\" service");
+ }
+ }
+
+#endif
+
+}
+
+CsoneControllerTab::~CsoneControllerTab()
+{
+ delete ui;
+}
+
+
+
+
+
+
+
+float CsoneControllerTab::validate_and_get_value_from_lineEdit(QLineEdit * lineEdit, float min, float max, int decimals, float default_value)
+{
+ // Initialise the value to the default
+ float return_value = default_value;
+
+ // Update the duration from the field
+ if(!lineEdit->text().isEmpty())
+ {
+ return_value = (lineEdit->text()).toFloat();
+ // Ensure that it is in the range [2,60]
+ if (return_value < min)
+ return_value = min;
+ else if (return_value > max)
+ return_value = max;
+ }
+
+ // Clip the value to the specified decimal places
+
+
+ // Put the value back into the line edit
+ lineEdit->setText(QString::number( return_value, 'f', decimals));
+
+ // Return the value
+ return return_value;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 CsoneControllerTab::on_perform_step_button_clicked()
+{
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Set the flag that a step should be performed
+ m_shouldPerformStep = true;
+
+ // Set the flag that the zoom levels should be reset
+ m_shouldResetZoom = true;
+
+ // Set the time back to be less than zero
+ m_time_for_step = -1.0;
+
+ // Set the minimum of the x-axis to agree with this
+ ui->chartView_for_x ->chart()->axisX()->setMin(m_time_for_step);
+ ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step);
+
+ // Update the duration from the field
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
+
+ // Set the minimum of the x-axis to agree with the duration
+ ui->chartView_for_x ->chart()->axisX()->setMax(m_step_response_data_recording_duration);
+ ui->chartView_for_pitch->chart()->axisX()->setMax(m_step_response_data_recording_duration);
+
+
+ // Clear any data from the line series
+ // > For x position
+ m_lineSeries_for_setpoint_x->removePoints(0,m_lineSeries_for_setpoint_x->count());
+ m_lineSeries_for_measured_x->removePoints(0,m_lineSeries_for_measured_x->count());
+ // > For pitch angles
+ //m_lineSeries_for_setpoint_pitch->removePoints(0,m_lineSeries_for_setpoint_pitch->count());
+ m_lineSeries_for_measured_pitch->removePoints(0,m_lineSeries_for_measured_pitch->count());
+
+ // Set all the fields to be blank
+ ui->lineEdit_step_start ->setText("---");
+ ui->lineEdit_step_end ->setText("---");
+ ui->lineEdit_overshoot_value ->setText("---");
+ ui->lineEdit_overshoot_percent ->setText("---");
+ ui->lineEdit_rise_time ->setText("---");
+ ui->lineEdit_settling_time ->setText("---");
+
+ // Set the flag that should store data
+ m_shouldStoreData_for_plotting = true;
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+ // Inform the user about the change
+#ifdef CATKIN_MAKE
+ ROS_INFO_STREAM("[CSONE CONTROLLER TAB GUI] Perform step started");
+#endif
+}
+
+void CsoneControllerTab::on_log_data_button_clicked()
+{
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Set the flag that a step should be performed
+ m_shouldPerformStep = false;
+
+ // Set the flag that the zoom levels should be reset
+ m_shouldResetZoom = true;
+
+ // Set the time back to zero
+ m_time_for_step = 0.0;
+
+ // Set the minimum of the x-axis to agree with this
+ ui->chartView_for_x ->chart()->axisX()->setMin(m_time_for_step);
+ ui->chartView_for_pitch->chart()->axisX()->setMin(m_time_for_step);
+
+ // Update the duration from the field
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
+
+ // Set the minimum of the x-axis to agree with the duration
+ ui->chartView_for_x ->chart()->axisX()->setMax(m_step_response_data_recording_duration);
+ ui->chartView_for_pitch->chart()->axisX()->setMax(m_step_response_data_recording_duration);
+
+ // Clear any data from the line series
+ // > For x position
+ m_lineSeries_for_setpoint_x->removePoints(0,m_lineSeries_for_setpoint_x->count());
+ m_lineSeries_for_measured_x->removePoints(0,m_lineSeries_for_measured_x->count());
+ // > For pitch angles
+ //m_lineSeries_for_setpoint_pitch->removePoints(0,m_lineSeries_for_setpoint_pitch->count());
+ m_lineSeries_for_measured_pitch->removePoints(0,m_lineSeries_for_measured_pitch->count());
+
+ // Set all the fields to be blank
+ ui->lineEdit_step_start ->setText("---");
+ ui->lineEdit_step_end ->setText("---");
+ ui->lineEdit_overshoot_value ->setText("---");
+ ui->lineEdit_overshoot_percent ->setText("---");
+ ui->lineEdit_rise_time ->setText("---");
+ ui->lineEdit_settling_time ->setText("---");
+
+ // Set the flag that should store data
+ m_shouldStoreData_for_plotting = true;
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+ // Inform the user about the change
+#ifdef CATKIN_MAKE
+ ROS_INFO_STREAM("[CSONE CONTROLLER TAB GUI] Perform step started");
+#endif
+}
+
+
+
+
+void CsoneControllerTab::on_simulate_step_response_button_clicked()
+{
+ // Initialise a flag for whether to start a simulation or not
+ bool shouldStartSimulation = true;
+
+ // Set the flag that a simulation is being performed
+ m_simulation_mutex.lock();
+ if (m_simulationIsInProgress)
+ {
+ shouldStartSimulation = false;
+ }
+ else
+ {
+ m_simulationIsInProgress = true;
+ // Set the button to not be avaialble
+ ui->simulate_step_response_button->setEnabled(false);
+ ui->clear_simulation_button->setEnabled(false);
+ }
+ m_simulation_mutex.unlock();
+
+ // Call the function that performs the simulation
+ if (shouldStartSimulation)
+ {
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Clear the line series
+ m_lineSeries_for_sim_setpoint_x->removePoints(0,m_lineSeries_for_sim_setpoint_x->count());
+ m_lineSeries_for_sim_measured_x->removePoints(0,m_lineSeries_for_sim_measured_x->count());
+ m_lineSeries_for_sim_measured_pitch->removePoints(0,m_lineSeries_for_sim_measured_pitch->count());
+
+ // > For x position
+ m_lineSeries_for_setpoint_x->removePoints(0,m_lineSeries_for_setpoint_x->count());
+ m_lineSeries_for_measured_x->removePoints(0,m_lineSeries_for_measured_x->count());
+ // > For pitch angles
+ //m_lineSeries_for_setpoint_pitch->removePoints(0,m_lineSeries_for_setpoint_pitch->count());
+ m_lineSeries_for_measured_pitch->removePoints(0,m_lineSeries_for_measured_pitch->count());
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+ simulate_step_response();
+ //QTimer::singleShot(50, this, SLOT(simulate_step_response()));
+ }
+}
+
+
+void CsoneControllerTab::on_clear_simulation_button_clicked()
+{
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // First clear the line series
+ m_lineSeries_for_sim_setpoint_x->removePoints(0,m_lineSeries_for_sim_setpoint_x->count());
+ m_lineSeries_for_sim_measured_x->removePoints(0,m_lineSeries_for_sim_measured_x->count());
+ m_lineSeries_for_sim_measured_pitch->removePoints(0,m_lineSeries_for_sim_measured_pitch->count());
+
+ // > For x position
+ m_lineSeries_for_setpoint_x->removePoints(0,m_lineSeries_for_setpoint_x->count());
+ m_lineSeries_for_measured_x->removePoints(0,m_lineSeries_for_measured_x->count());
+ // > For pitch angles
+ //m_lineSeries_for_setpoint_pitch->removePoints(0,m_lineSeries_for_setpoint_pitch->count());
+ m_lineSeries_for_measured_pitch->removePoints(0,m_lineSeries_for_measured_pitch->count());
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+}
+
+
+
+
+void CsoneControllerTab::on_set_lead_compensator_parameters_button_clicked()
+{
+ // Initialise local variable for each of (x,y,z,yaw)
+ float k = 1.0f, T = 1.0f, alpha = 1.0f;
+
+ // Lock the mutex
+ m_controller_parameter_mutex.lock();
+
+ // Take the new value if available, otherwise use default
+ // > For k
+ k = validate_and_get_value_from_lineEdit(ui->lineEdit_k,-100.0,100.0,4,0.016);
+
+ // > For T
+ T = validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,1,4.0);
+
+ // > For alpha
+ alpha = validate_and_get_value_from_lineEdit(ui->lineEdit_alpha,0.1,1.0,2,0.1);
+
+ // Unlock the mutex
+ m_controller_parameter_mutex.unlock();
+
+ // Call the function to publish the controller parameters
+ publishControllerParamters(k,T,alpha);
+}
+
+
+
+void CsoneControllerTab::on_set_time_delay_button_clicked()
+{
+ // Take the new value if available, otherwise use default
+ float time_delay_float = validate_and_get_value_from_lineEdit(ui->lineEdit_time_delay,0.0,1000.0,0,0.0);
+ float time_delay_int = int(time_delay_float);
+
+ // Call the function to publish the time delay
+ publishRequestForTimeDelayChange(time_delay_int);
+}
+
+
+
+void CsoneControllerTab::on_toggle_integrator_button_clicked()
+{
+ // Call the function to publish the request for a change in the integrator state
+ publishRequestForIntegratorStateChange(INTEGRATOR_FLAG_TOGGLE);
+}
+
+void CsoneControllerTab::on_reset_integrator_button_clicked()
+{
+ // Call the function to publish the request for a change in the integrator state
+ publishRequestForIntegratorStateChange(INTEGRATOR_FLAG_RESET);
+}
+
+
+
+
+
+void CsoneControllerTab::on_lineEdit_k_returnPressed()
+{
+ ui->set_lead_compensator_parameters_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_T_returnPressed()
+{
+ ui->set_lead_compensator_parameters_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_alpha_returnPressed()
+{
+ ui->set_lead_compensator_parameters_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_kp_returnPressed()
+{
+ ui->set_pd_controller_parameters_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_kd_returnPressed()
+{
+ ui->set_pd_controller_parameters_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_time_delay_returnPressed()
+{
+ ui->set_time_delay_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_step_size_returnPressed()
+{
+ ui->perform_step_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_step_duration_returnPressed()
+{
+ ui->log_data_button->animateClick();
+}
+
+
+
+
+void CsoneControllerTab::on_lineEdit_k_editingFinished()
+{
+ m_controller_parameter_mutex.lock();
+ validate_and_get_value_from_lineEdit(ui->lineEdit_k,-100.0,100.0,4,0.016);
+ m_controller_parameter_mutex.unlock();
+}
+
+void CsoneControllerTab::on_lineEdit_T_editingFinished()
+{
+ m_controller_parameter_mutex.lock();
+ validate_and_get_value_from_lineEdit(ui->lineEdit_T,0.0,100.0,1,4.0);
+ m_controller_parameter_mutex.unlock();
+}
+
+void CsoneControllerTab::on_lineEdit_alpha_editingFinished()
+{
+ m_controller_parameter_mutex.lock();
+ validate_and_get_value_from_lineEdit(ui->lineEdit_alpha,0.1,1.0,2,0.1);
+ m_controller_parameter_mutex.unlock();
+}
+
+
+
+void CsoneControllerTab::on_lineEdit_step_size_editingFinished()
+{
+ m_chart_mutex.lock();
+ validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,1,0.5);
+ m_chart_mutex.unlock();
+}
+
+void CsoneControllerTab::on_lineEdit_step_duration_editingFinished()
+{
+ m_chart_mutex.lock();
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
+ m_chart_mutex.unlock();
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 CsoneControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+{
+ // Check if the object is occluded for this data
+ if (!occluded)
+ {
+ // 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);
+ }
+
+ // Pass the data through to the plotting function
+ if (!occluded)
+ {
+ newDataForPerformingStepAndPlotting(x,pitch*RAD2DEG);
+ }
+ else
+ {
+ newDataForPerformingStepAndPlotting(0.0,0.0);
+ }
+}
+
+
+void CsoneControllerTab::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");
+}
+
+
+
+void CsoneControllerTab::newDataForPerformingStepAndPlotting(float x, float pitch)
+{
+ // Static variables for the min and max of the vertical axis
+ static float min_x_value_plotted = -0.01;
+ static float max_x_value_plotted = 0.01;
+ static float min_pitch_value_plotted = -0.01;
+ static float max_pitch_value_plotted = 0.01;
+ static float next_rezoom_at_time = 0.50;
+
+ // Initialise a bool for whether the step response should be analsysed
+ bool shouldAnalyseStepResponse = false;
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Reset the min, max, and rezoom time if requested
+ if (m_shouldResetZoom)
+ {
+ min_x_value_plotted = -0.01;
+ max_x_value_plotted = 0.01;
+ min_pitch_value_plotted = -0.01;
+ max_pitch_value_plotted = 0.01;
+ next_rezoom_at_time = 0.50;
+ // Set the flag to false
+ m_shouldResetZoom = false;
+ }
+
+ // Only do something if the flag indicates to do so
+ if (m_shouldStoreData_for_plotting)
+ {
+ // Add the data to the line series
+ // > For the x position
+ //float temp_x_setpoint = (ui->lineEdit_setpoint_current_x->text()).toFloat();
+ float temp_x_setpoint = m_current_setpoint_x;
+ m_lineSeries_for_setpoint_x->append(m_time_for_step, temp_x_setpoint );
+ m_lineSeries_for_measured_x->append(m_time_for_step, x );
+ // > For the pitch angle
+ //m_lineSeries_for_setpoint_pitch->append(m_time_for_step, ... );
+ m_lineSeries_for_measured_pitch->append(m_time_for_step, pitch );
+
+ // Update the min and max values
+ min_x_value_plotted = std::min( min_x_value_plotted , std::min(temp_x_setpoint,x) );
+ max_x_value_plotted = std::max( max_x_value_plotted , std::max(temp_x_setpoint,x) );
+ min_pitch_value_plotted = std::min( min_pitch_value_plotted , pitch );
+ max_pitch_value_plotted = std::max( max_pitch_value_plotted , pitch );
+
+ // Increment the time
+ m_time_for_step += 0.005;
+
+ // Change the setpoint if the time has been incremented to zero
+ // > Note: use 2 milliseconds as the threshold for zero
+ if (m_shouldPerformStep)
+ {
+ if (m_time_for_step >= -0.002)
+ {
+ // Set the flag so that we do not enter this loop again
+ m_shouldPerformStep = false;
+
+ // Extract the current step size
+ float step_size = validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,1,0.5);
+
+ // Determine the new x setpoint
+ float new_x = 0.0;
+ if (x < 0)
+ new_x = 0.5 * step_size;
+ else
+ new_x = -0.5 * step_size;
+
+ // Specify the default setpoints for (y,z,yaw)
+ float default_y = m_current_setpoint_y;
+ float default_z = m_current_setpoint_z;
+ float default_yaw = m_current_setpoint_yaw;
+
+ // Publish the new setpoint
+ publishSetpoint(new_x,default_y,default_z,default_yaw);
+ }
+ }
+
+ // Rezoom when the next time is reached
+ if ( m_time_for_step >= (next_rezoom_at_time+0.002) )
+ {
+ // Rezoom the x position chart
+ float diff_of_x_values_plotted = max_x_value_plotted - min_x_value_plotted;
+ ui->chartView_for_x->chart()->axisY()->setMin( min_x_value_plotted - 0.1*diff_of_x_values_plotted );
+ ui->chartView_for_x->chart()->axisY()->setMax( max_x_value_plotted + 0.1*diff_of_x_values_plotted );
+ // Rezoom the pitch angles chart
+ float diff_of_pitch_values_plotted = max_pitch_value_plotted - min_pitch_value_plotted;
+ ui->chartView_for_pitch->chart()->axisY()->setMin( min_pitch_value_plotted - 0.1*diff_of_pitch_values_plotted );
+ ui->chartView_for_pitch->chart()->axisY()->setMax( max_pitch_value_plotted + 0.1*diff_of_pitch_values_plotted );
+ // Increment the next rezoom time
+ next_rezoom_at_time += 0.5;
+ }
+
+ // Stop data collection and analyse the results if the time has
+ // passed "m_step_response_data_recording_duration" seconds
+ if (m_time_for_step >= (m_step_response_data_recording_duration+0.002))
+ {
+ // Set the flag to stop data collection
+ m_shouldStoreData_for_plotting = false;
+ // Rezoom the x position chart
+ float diff_of_x_values_plotted = max_x_value_plotted - min_x_value_plotted;
+ ui->chartView_for_x->chart()->axisY()->setMin( min_x_value_plotted - 0.1*diff_of_x_values_plotted );
+ ui->chartView_for_x->chart()->axisY()->setMax( max_x_value_plotted + 0.1*diff_of_x_values_plotted );
+ // Rezoom the pitch angle chart
+ float diff_of_pitch_values_plotted = max_pitch_value_plotted - min_pitch_value_plotted;
+ ui->chartView_for_pitch->chart()->axisY()->setMin( min_pitch_value_plotted - 0.1*diff_of_pitch_values_plotted );
+ ui->chartView_for_pitch->chart()->axisY()->setMax( max_pitch_value_plotted + 0.1*diff_of_pitch_values_plotted );
+ // Set the flag that the step response should be analysed
+ shouldAnalyseStepResponse = true;
+ // Inform the user that the step is finished
+ #ifdef CATKIN_MAKE
+ ROS_INFO("[CSONE CONTROLLER GUI] Step finished.");
+ #endif
+ }
+ }
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+ // Perform analysis of the step response if required
+ if (shouldAnalyseStepResponse)
+ {
+ analyseStepResponse();
+ }
+
+}
+
+void CsoneControllerTab::analyseStepResponse()
+{
+ // Initialise variable for the quantities to be determined
+ float setpoint_start = -999.9;
+ float setpoint_end = -999.9;
+ float rise_time = -999.9;
+ float overshoot_value = -999.9;
+ float settling_time = -999.9;
+ float overshoot_percent = -999.9;
+
+ // RETIEVE THE NECESSARY DATA INTO LOCAL VARIABLES
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Get the length of the line series
+ int count_of_lineSeries_for_setpoint_x = m_lineSeries_for_setpoint_x->count();
+ int count_of_lineSeries_for_measured_x = m_lineSeries_for_measured_x->count();
+
+ // Get all the points
+ QVector<QPointF> vector_of_points_for_setpoint_x = m_lineSeries_for_setpoint_x->pointsVector();
+ QVector<QPointF> vector_of_points_for_measured_x = m_lineSeries_for_measured_x->pointsVector();
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+ // PERFORM THE ANALYSIS
+
+ // Only do something if there is actually data to process
+ if ( (count_of_lineSeries_for_setpoint_x<1) && (count_of_lineSeries_for_measured_x<1) )
+ {
+ // Inform the user that the step is finished
+ #ifdef CATKIN_MAKE
+ ROS_ERROR("[CSONE CONTROLLER GUI] Request analyse of the step response, but the line series have a count of zero.");
+ #endif
+ // End this function
+ return;
+ }
+
+ // Get the start and end setpoint
+ if ( count_of_lineSeries_for_setpoint_x>0 )
+ {
+ // Get the start point
+ setpoint_start = vector_of_points_for_setpoint_x.at(0).y();
+ // Get the end point
+ setpoint_end = vector_of_points_for_setpoint_x.at(count_of_lineSeries_for_setpoint_x-1).y();
+ // DEBUGGING: print out the start and end values
+ #ifdef CATKIN_MAKE
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Setpoint at (start,end) = ( " << setpoint_start << " , " << setpoint_end << " )");
+ #endif
+ }
+
+
+ // Get the rise time, overshoot, and settling time
+ if ( (count_of_lineSeries_for_setpoint_x>0) && (count_of_lineSeries_for_measured_x>0) )
+ {
+ // Only continue if the step was at least 1 centimeter
+ if ( (setpoint_end-setpoint_start)<(-0.01) || (0.01)<(setpoint_end-setpoint_start) )
+ {
+ // Determine if it is a step up of a step down
+ float step_direction = 0.0;
+ if (setpoint_start<setpoint_end)
+ {
+ step_direction = 1.0;
+ }
+ else
+ {
+ step_direction = -1.0;
+ }
+
+ // Compute the threshold for the rise time
+ float rise_time_threshold = setpoint_start + 0.9*(setpoint_end-setpoint_start);
+
+ // Compute the threshold for the settling time
+ float settiling_time_upper_threshold = setpoint_end + step_direction * 0.05 * (setpoint_end-setpoint_start);
+ float settiling_time_lower_threshold = setpoint_end - step_direction * 0.05 * (setpoint_end-setpoint_start);
+
+ // Prepare a few variables needed
+ float this_time = 0.0;
+ float this_val = 0.0;
+ bool riseTimeFound = false;
+ overshoot_value = step_direction*(-100.0);
+
+ // Iterate through the points
+ for (int i = 0; i < vector_of_points_for_measured_x.size(); ++i)
+ {
+ // Get the values for this point
+ this_time = vector_of_points_for_measured_x.at(i).x();
+ this_val = vector_of_points_for_measured_x.at(i).y();
+
+ // Check if the rise time is exceeded
+ if (!riseTimeFound)
+ {
+ // Check if the rise time threshold is exceeded
+ if ( (step_direction*this_val) > (step_direction*rise_time_threshold) )
+ {
+ // Save this as the rise time
+ rise_time = this_time;
+ // Set the flag that it has been found
+ riseTimeFound = true;
+ }
+ }
+
+ // Keep track of the maximum value for the overshoot
+ overshoot_value = step_direction * std::max( step_direction*overshoot_value , step_direction*this_val );
+
+ // Check if outside the settling time threshold
+ if ( (this_val<settiling_time_lower_threshold) || (settiling_time_upper_threshold<this_val) )
+ {
+ // Update the settling time to the time for this iteration
+ settling_time = this_time;
+ }
+ }
+
+ // Convert the settling time to a percentage
+ overshoot_percent = (overshoot_value - setpoint_end) / (setpoint_end-setpoint_start) * 100.0;
+
+ // DEBUGGING: print out the start and end values
+ #ifdef CATKIN_MAKE
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] (rise time,overshoot value, os percent,settling time) = ( " << rise_time << " , " << overshoot_value << " , " << overshoot_percent << " , " << settling_time << " )");
+ #endif
+ }
+ }
+
+ // DISPLAY THE RESULTS IN THE FIELDS OF THE GUI
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Call the "update" function for each
+ updateStepAnalysisLineEdit(setpoint_start , ui->lineEdit_step_start , 3);
+ updateStepAnalysisLineEdit(setpoint_end , ui->lineEdit_step_end , 3);
+ updateStepAnalysisLineEdit(overshoot_value , ui->lineEdit_overshoot_value , 3);
+ updateStepAnalysisLineEdit(overshoot_percent , ui->lineEdit_overshoot_percent , 1);
+ updateStepAnalysisLineEdit(rise_time , ui->lineEdit_rise_time , 2);
+ updateStepAnalysisLineEdit(settling_time , ui->lineEdit_settling_time , 2);
+
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+
+}
+
+
+
+void CsoneControllerTab::updateStepAnalysisLineEdit(float value, QLineEdit * lineEdit, int num_dec_places)
+{
+ // Initialise a string variable for adding the "+"
+ QString qstr = "";
+
+ // Check if the value is "valid" or not
+ if (value > -999.0)
+ {
+ if (value < 0.0f) qstr = ""; else qstr = "+";
+ lineEdit->setText(qstr + QString::number( value, 'f', num_dec_places));
+ }
+ else
+ {
+ lineEdit->setText("---");
+ }
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 CsoneControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
+{
+ // EXTRACT THE SETPOINT
+ float x = newSetpoint.x;
+ float y = newSetpoint.y;
+ float z = newSetpoint.z;
+ float yaw = newSetpoint.yaw;
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Update the class variables for the current setpoint
+ m_current_setpoint_x = x;
+ m_current_setpoint_y = y;
+ m_current_setpoint_z = z;
+ m_current_setpoint_yaw = yaw;
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+}
+#endif
+
+
+
+
+#ifdef CATKIN_MAKE
+void CsoneControllerTab::integratorStateChangedCallback(const dfall_pkg::IntWithHeader& newIntegratorState)
+{
+ // EXTRACT THE NEW STATE
+ int new_state = newIntegratorState.data;
+
+ // Lock the mutex
+ //m_chart_mutex.lock();
+
+ switch (new_state)
+ {
+ case INTEGRATOR_FLAG_ON:
+ {
+ ui->label_integrator_state->setText("on");
+ break;
+ }
+ case INTEGRATOR_FLAG_OFF:
+ {
+ ui->label_integrator_state->setText("off");
+ break;
+ }
+ case INTEGRATOR_FLAG_UNKNOWN:
+ {
+ ui->label_integrator_state->setText("Unknown");
+ break;
+ }
+ default:
+ {
+ ui->label_integrator_state->setText("Not recognised");
+ break;
+ }
+ }
+
+ // Unlock the mutex
+ //m_chart_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 CsoneControllerTab::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("[CSONE 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) << "[CSONE CONTROLLER GUI] would publish request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]";
+#endif
+}
+
+
+
+
+
+void CsoneControllerTab::publishControllerParamters(float k, float T, float alpha)
+{
+#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 controller values
+ msg.x = k;
+ msg.y = T;
+ msg.z = alpha;
+
+ // Publish the setpoint
+ this->requestControllerParamterChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for new controller parameters (k,T,alpha) = (" << k << ", "<< T << ", "<< alpha << ")");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for: [" << k << ", "<< T << ", "<< alpha << "]";
+#endif
+
+ // Call the function to convert this to a discrete-time state-space controller
+ convertIntoDiscreteTimeParameters(k, T, alpha);
+}
+
+
+
+
+
+void CsoneControllerTab::publishRequestForIntegratorStateChange(int flag_to_publish)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::IntWithHeader msg;
+
+ // Fill the header of the message
+ fillIntMessageHeader( msg );
+
+ // Fill in the controller values
+ msg.data = flag_to_publish;
+
+ // Publish the setpoint
+ this->requestIntegratorStateChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for integrator state to change with flag " << flag_to_publish << ")");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for integrator state to change with flag " << flag_to_publish << ")";
+#endif
+}
+
+
+
+
+
+void CsoneControllerTab::publishRequestForTimeDelayChange(int time_delay_to_publish)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::IntWithHeader msg;
+
+ // Fill the header of the message
+ fillIntMessageHeader( msg );
+
+ // Fill in the controller values
+ msg.data = time_delay_to_publish;
+
+ // Publish the setpoint
+ this->requestTimeDelayChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for time delay of " << time_delay_to_publish << " milliseconds");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for time delay of " << time_delay_to_publish << " milliseconds" << "\n";
+#endif
+}
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
+void CsoneControllerTab::convertIntoDiscreteTimeParameters(float k, float T, float alpha)
+{
+ float control_frequency = 200.0;
+
+ if (alpha > 1){alpha = 1;} else if (alpha<0.1){alpha = 0.1;}
+
+ if (T > 100){T = 100;} else if (T<0.1){T = 0.1;}
+
+ // Lock the mutex
+ m_simulation_mutex.lock();
+
+ // Compute the A,B,C,D matrices
+ m_lead_compensator_A = pow(2.71828,(-1.0/(control_frequency*alpha*T)));
+ m_lead_compensator_B = -alpha*T*(m_lead_compensator_A-1.0);
+ m_lead_compensator_C = k/(alpha*T)*(1.0-1.0/alpha);
+ m_lead_compensator_D = k/alpha;
+
+ // Reset the state of the lead compensator to zero
+ m_lead_compensator_state = 0.0;
+
+#ifdef CATKIN_MAKE
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] Parameters changed to k=" << k << ", T=" << T << ", alpha=" << alpha << "\n";
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] Matrices changed to A=" << m_lead_compensator_A << ", B=" << m_lead_compensator_B << ", C=" << m_lead_compensator_C << ", D=" << m_lead_compensator_D;
+#endif
+
+ // Unlock the mutex
+ m_simulation_mutex.unlock();
+
+}
+
+
+
+
+void CsoneControllerTab::simulate_step_response()
+{
+ // GET THE CONTROLLER PARAMETERS INTO LOCAL VARIABLES
+
+ // Lock the mutex
+ m_simulation_mutex.lock();
+
+ float A = m_lead_compensator_A;
+ float B = m_lead_compensator_B;
+ float C = m_lead_compensator_C;
+ float D = m_lead_compensator_D;
+
+ float controller_state = 0.0;
+
+ // Unlock the mutex
+ m_simulation_mutex.unlock();
+
+ // GET THE STEP SPECIFICATIONS INTO LOCAL VARIABLES
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ float current_setpoint_x = m_current_setpoint_x;
+
+ float step_size = validate_and_get_value_from_lineEdit(ui->lineEdit_step_size,-2.0,2.0,1,0.5);
+
+ m_step_response_data_recording_duration = validate_and_get_value_from_lineEdit(ui->lineEdit_step_duration,2.0,20.0,0,10.0);
+ float duration = m_step_response_data_recording_duration;
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+
+
+ // Take the new value if available, otherwise use default
+ float time_delay_float = validate_and_get_value_from_lineEdit(ui->lineEdit_time_delay,0.0,1000.0,0,0.0);
+ float time_delay_int = int(time_delay_float);
+
+ // Compute the number of milliseconds per time step
+ float delta_T_in_milliseconds = 1000.0 / 200.0;
+
+ // Convert the time delay to a number of time steps
+ int time_delay_in_steps = int( (float(time_delay_int) + 0.1) / delta_T_in_milliseconds );
+
+ // Wrap this value into the allowed limits
+ if (time_delay_in_steps<0)
+ time_delay_in_steps=0;
+ if(time_delay_in_steps>(300-1))
+ time_delay_in_steps=300-1;
+#ifdef CATKIN_MAKE
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] time delay in steps = " << time_delay_in_steps << "\n";
+#endif
+
+
+ // PERFORM THE DURATION
+
+ // Initialise things
+ float control_frequency = 200.0;
+ float control_deltaT = 0.005;
+
+ float sim_time = 0.0;
+
+ int num_time_steps = int( duration / control_deltaT );
+
+ int num_point_per_append = 10;
+ int next_index = 0;
+
+ QVector<float> sim_traj_time (num_point_per_append,0);
+
+ //std::vector<float> sim_traj_x (num_time_steps,0);
+ QVector<float> sim_traj_x (num_point_per_append,0);
+ //QList<QPointF> sim_traj_x;
+
+ QVector<float> sim_traj_pitch (num_point_per_append,0);
+
+ // The inertial x measurement during a time window of 300 measurements
+ QVector<float> x_buffer (300,0);
+
+ float x_reference_start = current_setpoint_x;
+ // Determine the new x setpoint
+ float x_reference_end = 0.0;
+ if (x_reference_start < 0)
+ x_reference_end = 0.5 * step_size;
+ else
+ x_reference_end = -0.5 * step_size;
+
+ // Initialise the current state
+ float current_x = x_reference_start;
+ float current_xdot = 0.0;
+ float current_pitch = 0.0;
+
+ // Initialise variables for keeping track of the min and max
+ float min_x_value_sim = -0.01;
+ float max_x_value_sim = 0.01;
+ float min_pitch_value_sim = -0.01;
+ float max_pitch_value_sim = 0.01;
+
+ // Update the min and max
+ min_x_value_sim = std::min( min_x_value_sim , current_x );
+ max_x_value_sim = std::max( max_x_value_sim , current_x );
+ min_pitch_value_sim = std::min( min_pitch_value_sim , current_pitch*float(RAD2DEG) );
+ max_pitch_value_sim = std::max( max_pitch_value_sim , current_pitch*float(RAD2DEG) );
+
+
+ // Put in the initial conditions
+ sim_traj_time[next_index] = sim_time;
+
+ sim_traj_x[next_index] = current_x;
+ //sim_traj_x.append( QPointF(sim_time,current_x) );
+
+ sim_traj_pitch[next_index] = current_pitch;
+
+ next_index++;
+
+
+
+
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // For the x reference
+ m_lineSeries_for_sim_setpoint_x->append(0.0, x_reference_start );
+ m_lineSeries_for_sim_setpoint_x->append(0.0, x_reference_end );
+ m_lineSeries_for_sim_setpoint_x->append(duration, x_reference_end );
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+
+ int write_index = 0;
+ int read_index = 0;
+
+
+ // Iterate over the duration
+ for ( int itime=1 ; itime<num_time_steps ; itime++ )
+ {
+ // Increment the write index
+ write_index += 1;
+ // And wrap it back into range if necessary
+ if (write_index>=x_buffer.size())
+ {
+ write_index = 0;
+ }
+
+ // Compute the next read index based on the delay
+ read_index = write_index - time_delay_in_steps;
+ // And wrap it back into range if necessary
+ if (read_index<0)
+ {
+ read_index += x_buffer.size();
+ }
+
+ // Write the new data to the buffer
+ x_buffer[write_index] = current_x;
+
+ // Read the data for this time step from the buffer
+ float x_for_this_time_step = x_buffer[read_index];
+
+ // Compute the error for this time step
+ float x_error = x_reference_end - x_for_this_time_step;
+
+ // Compute the input to apply
+ // > FIRST: compute the new pitch reference
+ float pitch_ref = C*controller_state + D*x_error;
+ // > SECOND: evaluate the state update equation
+ controller_state = A*controller_state + B*x_error;
+ // > THIRD: perform the inner controller
+ float pitch_rate = 5.0*(pitch_ref-current_pitch);
+
+// A =
+// x1 x2 x3
+// x1 1 0.004998 0.0001226
+// x2 0 0.9993 0.04903
+// x3 0 0 1
+
+// B =
+// u1
+// x1 2.043e-07
+// x2 0.0001226
+// x3 0.005
+
+ // Perform the evolution of the state
+ current_x = current_x + 0.004998*current_xdot + 0.0001226*current_pitch;
+ current_xdot = 0.9993*current_xdot + 0.04903*current_pitch + 0.0001226*pitch_rate;
+ current_pitch = current_pitch + 0.005*pitch_rate;
+
+ // Update the min and max
+ min_x_value_sim = std::min( min_x_value_sim , current_x );
+ max_x_value_sim = std::max( max_x_value_sim , current_x );
+ min_pitch_value_sim = std::min( min_pitch_value_sim , current_pitch*float(RAD2DEG) );
+ max_pitch_value_sim = std::max( max_pitch_value_sim , current_pitch*float(RAD2DEG) );
+
+ // Increment the simulation time
+ sim_time += control_deltaT;
+
+ // Store the state and time
+ sim_traj_time[next_index] = sim_time;
+
+ sim_traj_x[next_index] = current_x;
+ //sim_traj_x.append( QPointF(sim_time,current_x) );
+
+ sim_traj_pitch[next_index] = current_pitch;
+
+ // Increment the next index pointer
+ next_index++;
+
+ // Update the chart if required
+ if (next_index >= num_point_per_append)
+ {
+ // Lock the mutex
+ m_chart_mutex.lock();
+ // Iterate through the point
+ for (int ipoint=0 ; ipoint<num_point_per_append ; ipoint++)
+ {
+ m_lineSeries_for_sim_measured_x->append( sim_traj_time[ipoint] , sim_traj_x[ipoint] );
+ m_lineSeries_for_sim_measured_pitch->append( sim_traj_time[ipoint] , sim_traj_pitch[ipoint]*RAD2DEG );
+
+ //ui->chartView_for_x->repaint();
+ }
+ next_index = 0;
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+ }
+ }
+
+
+ // PUT THE SIMULATION RESULTS INTO THE PLOTTED LINE SERIES
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Update the chart with any remaining points
+ if (next_index > 0)
+ {
+ // Iterate through the point
+ for (int ipoint=0 ; ipoint<next_index ; ipoint++)
+ {
+ m_lineSeries_for_sim_measured_x->append( sim_traj_time[ipoint] , sim_traj_x[ipoint] );
+ m_lineSeries_for_sim_measured_pitch->append( sim_traj_time[ipoint] , sim_traj_pitch[ipoint]*RAD2DEG );
+ }
+ next_index = 0;
+ }
+
+ // Rezoom the x position chart
+ float diff_of_x_values_sim = max_x_value_sim - min_x_value_sim;
+ ui->chartView_for_x->chart()->axisY()->setMin( min_x_value_sim - 0.1*diff_of_x_values_sim );
+ ui->chartView_for_x->chart()->axisY()->setMax( max_x_value_sim + 0.1*diff_of_x_values_sim );
+ // Rezoom the pitch angles chart
+ float diff_of_pitch_values_sim = max_pitch_value_sim - min_pitch_value_sim;
+ ui->chartView_for_pitch->chart()->axisY()->setMin( min_pitch_value_sim - 0.1*diff_of_pitch_values_sim );
+ ui->chartView_for_pitch->chart()->axisY()->setMax( max_pitch_value_sim + 0.1*diff_of_pitch_values_sim );
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+
+ // Set the flag that a simulation is completed
+ m_simulation_mutex.lock();
+ m_simulationIsInProgress = false;
+ // Set the button to not be avaialble
+ ui->simulate_step_response_button->setEnabled(true);
+ ui->clear_simulation_button->setEnabled(true);
+ m_simulation_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 CsoneControllerTab::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>("CsoneControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[CSONE 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("CsoneControllerService/SetpointChanged", 1, &CsoneControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // Lock the mutex
+ m_chart_mutex.lock();
+
+ // Set information back to the default
+ m_current_setpoint_x = 0.0;
+ m_current_setpoint_y = 0.0;
+ m_current_setpoint_z = 0.4;
+ m_current_setpoint_yaw = 0.0;
+
+ // Unlock the mutex
+ m_chart_mutex.unlock();
+
+ }
+#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 CsoneControllerTab::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("[CSONE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void CsoneControllerTab::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("[CSONE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void CsoneControllerTab::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("[CSONE 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 CsoneControllerTab::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("[CSONE 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("[CSONE 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("[CSONE CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[CSONE 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("[CSONE CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[CSONE 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("[CSONE 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/defaultcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
index 4170841a634141710fce641984b6e8f3f6dbf1ac..7904b231a353462038bc78e437a2ee8707ce4377 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
@@ -92,7 +92,7 @@ DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
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));
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
@@ -676,7 +676,7 @@ void DefaultControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool
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));
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
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
index 6a1e88f17690a7e56dd5701b4a434dee16f2ed98..8fc5ca827656558a05149d85dc0206b25868d239 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
@@ -120,6 +120,12 @@ void EnableControllerLoadYamlBar::showHideController_template_changed()
ui->load_yaml_template_button->setHidden( !(ui->load_yaml_template_button->isHidden()) );
}
+void EnableControllerLoadYamlBar::showHideController_csone_changed()
+{
+ ui->enable_csone_button ->setHidden( !(ui->enable_csone_button->isHidden()) );
+ ui->load_yaml_csone_button->setHidden( !(ui->load_yaml_csone_button->isHidden()) );
+}
+
// TEST MOTORS
@@ -206,6 +212,16 @@ void EnableControllerLoadYamlBar::on_enable_template_button_clicked()
#endif
}
+void EnableControllerLoadYamlBar::on_enable_csone_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_USE_CSONE_CONTROLLER;
+ this->commandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable CS1 Controller");
+#endif
+}
@@ -310,6 +326,21 @@ void EnableControllerLoadYamlBar::on_load_yaml_template_button_clicked()
#endif
}
+void EnableControllerLoadYamlBar::on_load_yaml_csone_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 = "CsoneController";
+ // 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 CS1 Controller YAML was clicked.");
+#endif
+}
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
index d6fc815ad0a3377c2e17a98b699c7b2130da6dc1..c29b832311aee31dfe59804f74483c6db8382c8d 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
@@ -149,6 +149,8 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ui->action_showHideController_remote->trigger();
// > For the template controller
ui->action_showHideController_template->trigger();
+ // > For the cs1 controller
+ ui->action_showHideController_csone->trigger();
}
@@ -261,6 +263,13 @@ void MainWindow::on_action_showHideController_template_changed()
ui->customWidget_controller_tabs->showHideController_template_changed();
}
+void MainWindow::on_action_showHideController_csone_changed()
+{
+ // Notify the UI elements of this change
+ ui->customWidget_enableControllerLoadYamlBar->showHideController_csone_changed();
+ ui->customWidget_controller_tabs->showHideController_csone_changed();
+}
+
void MainWindow::on_action_testMotors_triggered()
{
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
index f90b07f4c9ded2fb6ef36296e73d9baf71489cc1..0375c8f6901d39e108bcc4bff4a0c3fad2808c48 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
@@ -164,7 +164,7 @@ PickerControllerTab::PickerControllerTab(QWidget *parent) :
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));
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
@@ -1829,7 +1829,7 @@ void PickerControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
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));
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp
index b4dc5ecf60259e0a9be8cffe732188ef9516dfa9..38385a277d9f12e0c75f65616fc3d0fad972e4e5 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp
@@ -96,7 +96,7 @@ RemoteControllerTab::RemoteControllerTab(QWidget *parent) :
// ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("RemoteControllerService/GetCurrentSetpoint", false);
// dfall_pkg::GetSetpointService getSetpointCall;
// getSetpointCall.request.data = 0;
-// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1));
// if(getCurrentSetpointServiceClient.call(getSetpointCall))
// {
// setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
@@ -296,28 +296,6 @@ void RemoteControllerTab::publish_custom_button_command(int button_index , QLine
#endif
-void RemoteControllerTab::on_custom_button_1_clicked()
-{
-#ifdef CATKIN_MAKE
- //publish_custom_button_command(1,ui->lineEdit_custom_1);
-#endif
-}
-
-void RemoteControllerTab::on_custom_button_2_clicked()
-{
-#ifdef CATKIN_MAKE
- //publish_custom_button_command(2,ui->lineEdit_custom_2);
-#endif
-}
-
-void RemoteControllerTab::on_custom_button_3_clicked()
-{
-#ifdef CATKIN_MAKE
- //publish_custom_button_command(3,ui->lineEdit_custom_3);
-#endif
-}
-
-
@@ -604,7 +582,7 @@ void RemoteControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("RemoteControllerService/GetCurrentSetpoint", false);
dfall_pkg::GetSetpointService getSetpointCall;
getSetpointCall.request.data = 0;
- getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
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
index 440daaac14f06788852f06f82dad300ce0294510..64207a97bc6be97ef37307dffa36f2ffa8044cad 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
@@ -92,7 +92,7 @@ StudentControllerTab::StudentControllerTab(QWidget *parent) :
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));
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
@@ -713,7 +713,7 @@ void StudentControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool
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));
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
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
index b0da98f058131f3786ae24f33120be55ea9254aa..24f9fb07fedae441a7de3f94e035e811fc603062 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp
@@ -92,7 +92,7 @@ TemplateControllerTab::TemplateControllerTab(QWidget *parent) :
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));
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
@@ -465,7 +465,7 @@ void TemplateControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool
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));
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0));
if(getCurrentSetpointServiceClient.call(getSetpointCall))
{
setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
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
index 2b29975009c2617d65959f6a80f34e63232d5506..baa47e7b143d517fe4eb0bf530d660786450994d 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
@@ -93,7 +93,7 @@ TopBanner::TopBanner(QWidget *parent) :
ros::NodeHandle dfall_root_nodeHandle("/dfall");
// > Publisher for the emergency stop button
- emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("emergencyStop", 1);
+ 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);;
@@ -184,7 +184,7 @@ void TopBanner::loadCrazyflieContext(int ID_to_request_from_database , int emit_
contextCall.request.studentID = ID_to_request_from_database;
//ROS_INFO_STREAM("StudentID:" << m_agentID);
- centralManagerDatabaseService.waitForExistence(ros::Duration(2));
+ centralManagerDatabaseService.waitForExistence(ros::Duration(2.0));
if(centralManagerDatabaseService.call(contextCall))
{
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
index 6c1bf28f4abaa2a48a0923622eb95a70cf6769c5..f6578b3217df7ee6b9f04c7250610a5fb9e88b16 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
@@ -108,7 +108,7 @@ TuningControllerTab::TuningControllerTab(QWidget *parent) :
// 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));
+// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.1));
// if(getCurrentSetpointServiceClient.call(getSetpointCall))
// {
// setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
@@ -507,7 +507,7 @@ void TuningControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
// 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));
+// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(0.0));
// if(getCurrentSetpointServiceClient.call(getSetpointCall))
// {
// setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/forms/mainguiwindow.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/forms/mainguiwindow.ui
index 2d50c2a9dd741d735d515d406cda65846f8db683..5e470ee9fdb3e5964595d2ce5707699f57fa1dbc 100755
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/forms/mainguiwindow.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/forms/mainguiwindow.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1228</width>
- <height>1559</height>
+ <width>1339</width>
+ <height>1619</height>
</rect>
</property>
<property name="windowTitle">
@@ -116,6 +116,74 @@
</property>
</widget>
</item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QLabel" name="label_zmin">
+ <property name="text">
+ <string>zmin</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_zmin">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>-0.2</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QLabel" name="label_zmax">
+ <property name="text">
+ <string>zmax</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_zmax">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>1.8</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
</layout>
</widget>
</item>
@@ -972,8 +1040,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1228</width>
- <height>40</height>
+ <width>1339</width>
+ <height>47</height>
</rect>
</property>
</widget>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
index bfa475fd57e66806f9a3a4b1b29b108239029031..5d17177cbf0fff1ceafa4143166a2ed65946b9c7 100755
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
@@ -38,6 +38,7 @@
#include <QTimer>
#include <QGridLayout>
#include <QGraphicsRectItem>
+#include <QLineEdit>
#ifdef CATKIN_MAKE
@@ -131,6 +132,8 @@ private slots:
#ifdef CATKIN_MAKE
#endif
+ float validate_and_get_value_from_lineEdit(QLineEdit * lineEdit, float min, float max, int decimals, float default_value);
+
void doNumCrazyFlyZonesChanged(int n);
void transitionToMode(int mode);
void on_removeTable_clicked();
@@ -188,6 +191,10 @@ private slots:
// For the emergency stop button
void on_emergency_stop_button_clicked();
+
+ // For automatically validating the zmin and zmax line edits
+ void on_lineEdit_zmin_editingFinished();
+ void on_lineEdit_zmax_editingFinished();
private:
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
index 1c6173bfed1383d55b84f8f2929f24e321fd8922..a93637d1cd0e41b52681f7678764f7596ae011ff 100755
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/src/mainguiwindow.cpp
@@ -85,6 +85,33 @@ MainGUIWindow::~MainGUIWindow()
delete ui;
}
+float MainGUIWindow::validate_and_get_value_from_lineEdit(QLineEdit * lineEdit, float min, float max, int decimals, float default_value)
+{
+ // Initialise the value to the default
+ float return_value = default_value;
+
+ // Check that the field is NOT empty
+ if(!lineEdit->text().isEmpty())
+ {
+ // Extraxt the value from the field as a float
+ return_value = (lineEdit->text()).toFloat();
+ // Ensure that it is in the range specified
+ if (return_value < min)
+ return_value = min;
+ else if (return_value > max)
+ return_value = max;
+ }
+
+ // Clip the value to the specified decimal places
+
+
+ // Put the value back into the line edit
+ lineEdit->setText(QString::number( return_value, 'f', decimals));
+
+ // Return the value
+ return return_value;
+}
+
int MainGUIWindow::getTabIndexFromName(QString name)
{
int found_name = -1;
@@ -279,7 +306,7 @@ void MainGUIWindow::_init()
// 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);
+ emergencyStopPublisher = nodeHandle_dfall_root.advertise<dfall_pkg::IntWithHeader>("EmergencyStop", 1);
#endif
}
@@ -841,6 +868,10 @@ void MainGUIWindow::on_unlink_button_clicked()
void MainGUIWindow::on_save_in_DB_button_clicked()
{
+ // Get the zmin and zmax values from the line edits
+ float current_zmin = validate_and_get_value_from_lineEdit(ui->lineEdit_zmin,-10.0,10.0,2,-0.2);
+ float current_zmax = validate_and_get_value_from_lineEdit(ui->lineEdit_zmax,-10.0,10.0,2,1.8);
+
#ifdef CATKIN_MAKE
// we need to update and then save?
CrazyflieDB tmp_db;
@@ -867,8 +898,8 @@ void MainGUIWindow::on_save_in_DB_button_clicked()
tmp_entry.crazyflieContext.localArea.ymin = y_min * FROM_UNITS_TO_METERS;
tmp_entry.crazyflieContext.localArea.ymax = y_max * FROM_UNITS_TO_METERS;
- tmp_entry.crazyflieContext.localArea.zmin = -0.2;
- tmp_entry.crazyflieContext.localArea.zmax = 2.2;
+ tmp_entry.crazyflieContext.localArea.zmin = current_zmin;
+ tmp_entry.crazyflieContext.localArea.zmax = current_zmax;
}
}
tmp_db.crazyflieEntries.push_back(tmp_entry);
@@ -1104,3 +1135,23 @@ void MainGUIWindow::on_emergency_stop_button_clicked()
emergencyStopPublisher.publish(msg);
#endif
}
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+void MainGUIWindow::on_lineEdit_zmin_editingFinished()
+{
+ validate_and_get_value_from_lineEdit(ui->lineEdit_zmin,-10.0,10.0,2,-0.2);
+}
+
+void MainGUIWindow::on_lineEdit_zmax_editingFinished()
+{
+ validate_and_get_value_from_lineEdit(ui->lineEdit_zmax,-10.0,10.0,2,1.8);
+}
+
diff --git a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
index 3345d60630b2e7832208a8a8eda1989d4c244cd7..25d02f977e2b75257fa726e6d786869bdae6104a 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
@@ -413,9 +413,9 @@ if __name__ == '__main__':
# 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)
+ 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)
+ rospy.Subscriber("/dfall/coord" + coordID_as_string + "/FlyingAgentClient/CrazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback)
# Advertise a Serice for the current status
diff --git a/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py b/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
index f955bd2c0388996340f9729b490474e10be1aa58..9598f8522e074e78f73cda20bb755eeb318cdf9e 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
@@ -223,7 +223,7 @@ if __name__ == '__main__':
global cf_client
cf_client = CrazyRadioClient()
- # rospy.Subscriber("FlyingAgentClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
+ # rospy.Subscriber("FlyingAgentClient/CrazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts
# time.sleep(1.0)
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
index f1114564b25b3ee94b149b3fa019231bef8c8f2b..c28c297d6b8a6f41da9d0e8f18eaf8a6f54e7657 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
@@ -131,6 +131,7 @@
#define TUNING_CONTROLLER 6
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
+#define CSONE_CONTROLLER 9
#define TESTMOTORS_CONTROLLER 21
@@ -144,6 +145,7 @@
#define CMD_USE_TUNING_CONTROLLER 6
#define CMD_USE_PICKER_CONTROLLER 7
#define CMD_USE_TEMPLATE_CONTROLLER 8
+#define CMD_USE_CSONE_CONTROLLER 9
#define CMD_CRAZYFLY_TAKE_OFF 11
@@ -219,6 +221,24 @@
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// These constants deine the current and interaction with an intergrator
+#define INTEGRATOR_FLAG_ON 1
+#define INTEGRATOR_FLAG_OFF 2
+#define INTEGRATOR_FLAG_UNKNOWN 3
+#define INTEGRATOR_FLAG_TOGGLE 4
+#define INTEGRATOR_FLAG_RESET 5
+
+
+
+
// ----------------------------------------------------------------------------------
//
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..cdaa0bfd6d967b3acf51c11da88505a0fc629a5f
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -0,0 +1,292 @@
+// 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 CS1 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/IntIntService.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};
+
+// Integrator gains
+float yaml_integratorGain_forThrust = 0.0;
+float yaml_integratorGain_forTauXY = 0.0;
+float yaml_integratorGain_forTauYaw = 0.0;
+
+// The state of the integrator
+int m_integratorState = INTEGRATOR_FLAG_OFF;
+
+// The current value of the integrator for each axis
+float m_newtons_intergal = 0.0;
+float m_tau_x_integral = 0.0;
+float m_tau_y_integral = 0.0;
+float m_tau_z_integral = 0.0;
+
+
+
+// The weight of the Crazyflie in Newtons, i.e., mg
+float m_cf_weight_in_newtons = 0.0;
+
+// The state space matrices of lead compensator controller
+float m_A=0.967216;
+float m_B=0.00491758;
+float m_C=-12;
+float m_D=2;
+
+// The state of lead compensator controller
+float m_controller_state = 0.0;
+
+// The inertial x measurement during a time window of 300 measurements
+std::vector<float> m_x_inertial_buffer (300,0);
+
+// The time delay as an integer of how many measurements to delay by
+int m_time_delay_in_steps = 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 informing the network about
+// changes to the setpoint
+ros::Publisher m_setpointChangedPublisher;
+
+// ROS Publisher for informing the network about
+// changes to the integrator state
+ros::Publisher m_integratorStateChangedPublisher;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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);
+
+// REQUEST INTEGRATOR STATE CHANGE CALLBACK
+void requestIntegratorStateChangeCallback(const IntWithHeader& msg);
+
+// CHANGE INTEGRATOR STATE FUNCTION
+void setNewIntegratorState(int newIntegratorState);
+
+// GET CURRENT INTEGRATOR STATE SERVICE CALLBACK
+bool getCurrentIntegratorStateCallback(IntIntService::Request &request, IntIntService::Response &response);
+
+// REQUEST CONTROLLER PARAMETERS CALLBACK
+void requestTimeDelayChangeCallback(const IntWithHeader& msg);
+
+// CHANGE TIME DELAY FUNCTION
+void setNewTimeDelay(int newTimeDelay);
+
+// REQUEST CONTROLLER PARAMETERS CALLBACK
+void requestControllerParametersChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
+void convertIntoDiscreteTimeParameters(float k, float T, float alpha);
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
+
+// FOR LOADING THE YAML PARAMETERS
+void timerCallback_initial_load_yaml(const ros::TimerEvent&);
+void isReadyCsoneControllerYamlCallback(const IntWithHeader & msg);
+void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ 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
index 0bd861de6bfa058756c7cf8a8c5b7ab9f134eebf..a838f2f2b2a8b8ca5ea03ede4742ae9f56b9c5f3 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -146,9 +146,9 @@ using namespace dfall_pkg;
// These constants deine the behaviour of the intergrator
-#define INTEGRATOR_FLAG_ON 1
-#define INTEGRATOR_FLAG_OFF 2
-#define INTEGRATOR_FLAG_RESET 3
+#define DEFAULT_INTEGRATOR_FLAG_ON 1
+#define DEFAULT_INTEGRATOR_FLAG_OFF 2
+#define DEFAULT_INTEGRATOR_FLAG_RESET 3
@@ -426,7 +426,7 @@ bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Re
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
// > For the normal state
-void computeResponse_for_normal(Controller::Response &response);
+void computeResponse_for_normal(bool isFirstControllerCall, Controller::Response &response);
// > For the standby state (also used for unknown state)
void computeResponse_for_standby(Controller::Response &response);
// > For the take-off phases
@@ -449,8 +449,8 @@ void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controlle
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate);
// PUBLISHING OF THE DEBUG MESSAGE
void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index b41dee020f20f6f27ca04ab41d46919cc14b35ab..61b6d33059caac567b9d635401da2ecb23fd3a4f 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -175,15 +175,18 @@ ros::Timer m_timer_land_complete;
// VARIABLES RELATING TO CONTROLLER SELECTION
+// > Boolean indicating that the controller service clients
+// have been successfully created
+bool m_controllers_avialble = false;
// > 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;
+// > Boolean indicating that this is the first call to
+// the instant controller
+bool m_isFirstCall_to_instant_controller = false;
// > Timer for creating the controller service client after
// some delay
ros::Timer m_timer_for_createAllControllerServiceClients;
@@ -208,6 +211,8 @@ ros::ServiceClient m_pickerController;
ros::ServiceClient m_remoteController;
// The Template controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_templateController;
+// The CS1 controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_csoneController;
// The Test Mtoros controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_testMotorsController;
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
index edb52d6f0b15098aa3587e06c08a17d3cd05500d..a4c09f19bf7b5e755e8493c5ddf3d109d2042d3d 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/PickerControllerService.h
@@ -345,8 +345,8 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate);
// PUBLISHING OF THE DEBUG MESSAGE
void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
index 2b9e5899cd5a85fdc4a980e49f914976b71d0c7b..a47c52b54922052e5da14b3eb3656cabaeba1d18 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
@@ -274,7 +274,7 @@ float gravity_force_quarter = 0.25 * gravity_force;
// VARIABLES FOR THE CONTROLLER
// Frequency at which the controller is running
-float vicon_frequency = 200.0;
+float yaml_vicon_frequency = 200.0;
// Frequency at which the controller is running
float control_frequency = 200.0;
@@ -324,29 +324,29 @@ float cmd_sixteenbit_max = 60000;
// VARIABLES FOR THE ESTIMATOR
// Frequency at which the controller is running
-float estimator_frequency = 200.0;
+float m_estimator_frequency = 200.0;
// > A flag for which estimator to use:
-int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
// > The current state interial estimate,
// for use by the controller
-float current_stateInertialEstimate[12];
+float m_current_stateInertialEstimate[12];
// > The measurement of the Crazyflie at the "current" time step,
// to avoid confusion
-float current_xzy_rpy_measurement[6];
+float m_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];
+float m_previous_xzy_rpy_measurement[6];
// > The full 12 state estimate maintained by the finite
// difference state estimator
-float stateInterialEstimate_viaFiniteDifference[12];
+float m_stateInterialEstimate_viaFiniteDifference[12];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
-float stateInterialEstimate_viaPointMassKalmanFilter[12];
+float m_stateInterialEstimate_viaPointMassKalmanFilter[12];
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
@@ -451,8 +451,8 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12]
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate);
// PUBLISHING OF THE DEBUG MESSAGE
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
index 6172fb8643956c747944de11d42c5ef1b406485b..5534b1baa30f246f1f12df48770bcb77b184bc5f 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
@@ -288,7 +288,7 @@ float gravity_force_quarter;
// VARIABLES FOR THE CONTROLLER
// Frequency at which the controller is running
-float vicon_frequency;
+float yaml_vicon_frequency;
// Frequency at which the controller is running
float control_frequency;
@@ -362,29 +362,29 @@ float cmd_sixteenbit_max;
// VARIABLES FOR THE ESTIMATOR
// Frequency at which the controller is running
-float estimator_frequency;
+float m_estimator_frequency;
// > A flag for which estimator to use:
-int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
// > The current state interial estimate,
// for use by the controller
-float current_stateInertialEstimate[12];
+float m_current_stateInertialEstimate[12];
// > The measurement of the Crazyflie at the "current" time step,
// to avoid confusion
-float current_xzy_rpy_measurement[6];
+float m_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];
+float m_previous_xzy_rpy_measurement[6];
// > The full 12 state estimate maintained by the finite
// difference state estimator
-float stateInterialEstimate_viaFiniteDifference[12];
+float m_stateInterialEstimate_viaFiniteDifference[12];
// > The full 12 state estimate maintained by the point mass
// kalman filter state estimator
-float stateInterialEstimate_viaPointMassKalmanFilter[12];
+float m_stateInterialEstimate_viaPointMassKalmanFilter[12];
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
@@ -489,8 +489,8 @@ void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12]
// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate);
// PUBLISHING OF THE DEBUG MESSAGE
diff --git a/dfall_ws/src/dfall_pkg/launch/agent.launch b/dfall_ws/src/dfall_pkg/launch/agent.launch
index 0815543d28bece2e63ddf5929e93f3a6126fc254..cccd6020e3fc39aedd190ba90a19c3bdce133683 100755
--- a/dfall_ws/src/dfall_pkg/launch/agent.launch
+++ b/dfall_ws/src/dfall_pkg/launch/agent.launch
@@ -120,6 +120,15 @@
>
</node>
+ <!-- CSONE CONTROLLER -->
+ <node
+ pkg = "dfall_pkg"
+ name = "CsoneControllerService"
+ output = "screen"
+ type = "CsoneControllerService"
+ >
+ </node>
+
<!-- TEST MOTORS CONTROLLER -->
<node
pkg = "dfall_pkg"
diff --git a/dfall_ws/src/dfall_pkg/launch/coordinator.launch b/dfall_ws/src/dfall_pkg/launch/coordinator.launch
index c9e7e15414acb8efb1d0361afd4768eb2f8e2ae1..30fa23c456f006a6916aa9c44aef51d7144dd5e9 100755
--- a/dfall_ws/src/dfall_pkg/launch/coordinator.launch
+++ b/dfall_ws/src/dfall_pkg/launch/coordinator.launch
@@ -12,7 +12,7 @@
<!-- Example of how to specify the coordID from command line -->
<!-- roslaunch dfall_pkg coordID:=001 -->
- <group ns="coord$(arg coordID)">
+ <group ns="$(eval 'coord' + str(coordID).zfill(3))">
<!-- COORDINATOR GUI -->
<group if="$(arg withGUI)">
diff --git a/dfall_ws/src/dfall_pkg/param/BatteryMonitor.yaml b/dfall_ws/src/dfall_pkg/param/BatteryMonitor.yaml
index 3aeb59762f27449e478446117c073a1361425224..21666fc3d84f485f1519f27931d499d64997ab69 100755
--- a/dfall_ws/src/dfall_pkg/param/BatteryMonitor.yaml
+++ b/dfall_ws/src/dfall_pkg/param/BatteryMonitor.yaml
@@ -6,7 +6,7 @@ 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_lower_while_flying: 2.70
battery_voltage_threshold_upper_while_flying: 3.70
# Delay before changing the state of the battery, in [number of measurements]
diff --git a/dfall_ws/src/dfall_pkg/param/CsoneController.yaml b/dfall_ws/src/dfall_pkg/param/CsoneController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..fd02486a06e90856f679f5b1c7aa693079690e44
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/CsoneController.yaml
@@ -0,0 +1,33 @@
+# Mass of the crazyflie
+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
+
+# 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,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00]
+gainMatrixPitchRate : [ 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00]
+gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
+
+# Integrator gains
+integratorGain_forThrust : 0.10
+integratorGain_forTauXY : 0.06
+integratorGain_forTauYaw : 0.05
diff --git a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
index e6bacb8f6af079883950dd1df95c9b4a49056ca3..6edb428394692da236ffb24a4c522657fb19c334 100755
--- a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
@@ -6,6 +6,7 @@ tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
remoteController: "RemoteControllerService/RemoteController"
templateController: "TemplateControllerService/TemplateController"
+csoneController: "CsoneControllerService/CsoneController"
testMotorsController: "TestMotorsControllerService/TestMotorsController"
diff --git a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
index 4741dde7ffc5fafbbd15e1499d43a1e5bcb5dd48..ff922c8fb7b3048a4ff5003280478d36cef5cf2a 100644
--- a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
@@ -453,7 +453,7 @@ void QuadrotorSimulator::subscribe_to_commanding_agent_id( int commanding_agent_
// Subscribe to the flying state updates of the
// commanding agent
- flyingStateUpdatesOfCommandingAgentSubscriber = nodeHandle.subscribe("/dfall/agent" + commanding_agent_id_as_string + "/FlyingAgentClient/flyingState", 1, &QuadrotorSimulator::flying_state_update_of_commanding_agent_callback, this );
+ flyingStateUpdatesOfCommandingAgentSubscriber = nodeHandle.subscribe("/dfall/agent" + commanding_agent_id_as_string + "/FlyingAgentClient/FlyingState", 1, &QuadrotorSimulator::flying_state_update_of_commanding_agent_callback, this );
// Inform to user
ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" now subscribed to commands from agent with ID " << commanding_agent_id_as_string);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
index bf0af4453af893b840ef497f19ff3a472912587e..93f5a305abf457a3f457884c29998b656dedb357 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
@@ -523,7 +523,7 @@ int main(int argc, char* argv[])
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);
+ ros::Subscriber agentOperatingStateSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("FlyingState", 1, agentOperatingStateCallback);
// Initialise the operating state
m_agent_operating_state = AGENT_OPERATING_STATE_MOTORS_OFF;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
index 19fb02d244743c1355839469b0e3402f2bcb3161..f056425244ab54703064bec427a62bf31c07f615 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
@@ -534,9 +534,9 @@ int main(int argc, char* argv[])
// Subscriber to the commands for when to (dis-)connect the
// CrazyRadio connection with the Crazyflie
// > For the radio commands from the FlyingAgentClient of this agent
- ros::Subscriber crazyRadioCommandSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("crazyRadioCommand", 1, crazyRadioCommandCallback);
+ ros::Subscriber crazyRadioCommandSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("CrazyRadioCommand", 1, crazyRadioCommandCallback);
// > For the radio command from the coordinator
- ros::Subscriber crazyRadioCommandFromCoordSubscriber = nodeHandle_to_coordinator.subscribe("FlyingAgentClient/crazyRadioCommand", 1, crazyRadioCommandCallback);
+ ros::Subscriber crazyRadioCommandFromCoordSubscriber = nodeHandle_to_coordinator.subscribe("FlyingAgentClient/CrazyRadioCommand", 1, crazyRadioCommandCallback);
// Subscriber for the "Control Commands" received from the Flying Agent Client
ros::Subscriber controlCommandsSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("ControlCommand", 1, controlCommandCallback);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc508b989834143b6e661078d137f67a7e2b62f5
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -0,0 +1,1303 @@
+// 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 Csone Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/CsoneControllerService.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 "CsoneController"
+// 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
+
+ // Initialise static variables for handling the circular buffer
+ // of inertial x measurement
+ static int write_index = 0;
+ static int read_index = 0;
+
+ // Increment the write index
+ write_index += 1;
+ // And wrap it back into range if necessary
+ if (write_index>=m_x_inertial_buffer.size())
+ {
+ write_index = 0;
+ }
+
+ // Compute the next read index based on the delay
+ read_index = write_index - m_time_delay_in_steps;
+ // And wrap it back into range if necessary
+ if (read_index<0)
+ {
+ read_index += m_x_inertial_buffer.size();
+ }
+
+ // Write the new data to the buffer
+ m_x_inertial_buffer[write_index] = request.ownCrazyflie.x;
+
+ // Read the data for this time step from the buffer
+ float inertial_x_for_this_time_step = m_x_inertial_buffer[read_index];
+
+
+ // Define a local array to fill in with the state error
+ float stateErrorInertial[9];
+
+ // Fill in the (x,y,z) position error
+ stateErrorInertial[0] = inertial_x_for_this_time_step - 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
+ // > But only if this is NOT the first call to the controller
+ if (!request.isFirstControllerCall)
+ {
+ // > Compute 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;
+ }
+ else
+ {
+ // Set the velocities to zero
+ stateErrorInertial[3] = 0.0;
+ stateErrorInertial[4] = 0.0;
+ stateErrorInertial[5] = 0.0;
+ }
+
+ // 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];
+ }
+
+
+
+ // For the x-controller:
+ // > Execute the discrete-time state space equivalent of the lead
+ // compensator specified.
+ // > NOTE: the new pitch reference, i.e., the output to the state
+ // space, MUST be calucated before the state update equation
+ // is evaulated, in equations:
+ // FIRST: y(t) = C x(t) + D u(t)
+ // SECOND: x(t+1) = A x(t) + B u(t)
+
+ // > FIRST: compute the new pitch reference
+ float pitch_ref = m_C*m_controller_state - m_D*stateErrorBody[0];
+
+ // > SECOND: evaluate the state update equation
+ m_controller_state = m_A*m_controller_state - m_B*stateErrorBody[0];
+
+ // > THIRD: perform the inner controller
+ pitchRate_forResponse = 5.0*(pitch_ref-request.ownCrazyflie.pitch);
+
+ // 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;
+
+
+
+
+ // PERFORM THE INTEGRATOR COMPUTATIONS
+ if (m_integratorState == INTEGRATOR_FLAG_ON)
+ {
+ m_newtons_intergal -= yaml_integratorGain_forThrust * stateErrorBody[2] / yaml_control_frequency;
+ m_tau_x_integral += yaml_integratorGain_forTauXY * stateErrorBody[1] / yaml_control_frequency;
+ m_tau_y_integral -= yaml_integratorGain_forTauXY * stateErrorBody[0] / yaml_control_frequency;
+ m_tau_z_integral -= yaml_integratorGain_forTauYaw * stateErrorBody[8] / yaml_control_frequency;
+ }
+
+ float cmd1_integral_adjustments = m_newtons_intergal - m_tau_x_integral - m_tau_y_integral - m_tau_z_integral;
+ float cmd2_integral_adjustments = m_newtons_intergal - m_tau_x_integral + m_tau_y_integral + m_tau_z_integral;
+ float cmd3_integral_adjustments = m_newtons_intergal + m_tau_x_integral + m_tau_y_integral - m_tau_z_integral;
+ float cmd4_integral_adjustments = m_newtons_intergal + m_tau_x_integral - m_tau_y_integral + m_tau_z_integral;
+
+ // 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 + cmd1_integral_adjustments);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrust_request_per_motor + cmd2_integral_adjustments);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_request_per_motor + cmd3_integral_adjustments);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_request_per_motor + cmd4_integral_adjustments);
+
+
+ // 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;
+}
+
+
+
+
+
+// REQUEST INTEGRATOR STATE CHANGE CALLBACK
+void requestIntegratorStateChangeCallback(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 request change
+ int requested_change = msg.data;
+
+ // Switch between the possible cases
+ switch (requested_change)
+ {
+ case INTEGRATOR_FLAG_ON:
+ setNewIntegratorState(INTEGRATOR_FLAG_ON);
+ break;
+
+ case INTEGRATOR_FLAG_OFF:
+ setNewIntegratorState(INTEGRATOR_FLAG_OFF);
+ break;
+
+ case INTEGRATOR_FLAG_UNKNOWN:
+ ROS_INFO("[CSONE CONTROLLER] Does not respond for the integrator to change to state INTEGRATOR_FLAG_UNKNOWN");
+ break;
+
+ case INTEGRATOR_FLAG_TOGGLE:
+ {
+ if (m_integratorState==INTEGRATOR_FLAG_OFF)
+ setNewIntegratorState(INTEGRATOR_FLAG_ON);
+ else
+ setNewIntegratorState(INTEGRATOR_FLAG_OFF);
+ break;
+ }
+
+ case INTEGRATOR_FLAG_RESET:
+ // Reset the integrator value to zero
+ m_newtons_intergal = 0.0;
+ m_tau_x_integral = 0.0;
+ m_tau_y_integral = 0.0;
+ m_tau_z_integral = 0.0;
+ // Inform the user
+ ROS_INFO("[CSONE CONTROLLER] integrator values were reset to zero.");
+ break;
+
+ default:
+ ROS_INFO_STREAM("[CSONE CONTROLLER] The requested integrator state change of " << requested_change << " is not recognised.");
+ break;
+ }
+ }
+}
+
+// CHANGE INTEGRATOR STATE FUNCTION
+void setNewIntegratorState(int newIntegratorState){
+ // Directly set this to the class variable
+ m_integratorState = newIntegratorState;
+
+ // 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 "IntWithHeader"
+ IntWithHeader msg;
+ // Fill in the integrator state
+ msg.data = m_integratorState;
+ // Publish the message
+ m_integratorStateChangedPublisher.publish(msg);
+}
+
+
+
+
+// GET CURRENT INTEGRATOR STATE SERVICE CALLBACK
+bool getCurrentIntegratorStateCallback(IntIntService::Request &request, IntIntService::Response &response)
+{
+ // Directly put the current integrator state into the response
+ response.data = m_integratorState;
+ // Return
+ return true;
+}
+
+
+// REQUEST CONTROLLER PARAMETERS CALLBACK
+void requestTimeDelayChangeCallback(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 request change
+ int requested_time_delay = msg.data;
+
+ // Call the function for setting the time delay
+ setNewTimeDelay(requested_time_delay);
+ }
+}
+
+// CHANGE TIME DELAY FUNCTION
+void setNewTimeDelay(int newTimeDelay)
+{
+
+ // ASSUMPTION: that the time delay is in MILLISECONDS
+
+ // Compute the number of milliseconds per time step
+ float delta_T_in_milliseconds = 1000.0 / yaml_control_frequency;
+
+ // Convert the time delay to a number of time steps
+ int time_delay_in_steps = int( (float(newTimeDelay) + 0.1) / delta_T_in_milliseconds );
+
+ // Wrap this value into the allowed limits
+ if (time_delay_in_steps<0)
+ time_delay_in_steps=0;
+ if(time_delay_in_steps>(m_x_inertial_buffer.size()-1))
+ time_delay_in_steps=m_x_inertial_buffer.size()-1;
+
+ // Put the time delay into the class variable
+ m_time_delay_in_steps = time_delay_in_steps;
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Time delay changed to " << time_delay_in_steps << " steps, i.e., " << float(time_delay_in_steps)*1000.0/yaml_control_frequency << " milliseconds" );
+}
+
+
+// REQUEST CONTROLLER PARAMETERS CHANGE CALLBACK
+// Using x y z from SetpointWithHeader for k T alpha controller parameters
+void requestControllerParametersChangeCallback(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)
+ {
+ convertIntoDiscreteTimeParameters(newSetpoint.x, newSetpoint.y, newSetpoint.z);
+ m_controller_state = 0.0;
+ }
+}
+
+// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
+void convertIntoDiscreteTimeParameters(float k, float T, float alpha)
+{
+ if (alpha > 1){alpha = 1;} else if (alpha<0.1){alpha = 0.1;}
+
+ if (T > 100){T = 100;} else if (T<0.1){T = 0.1;}
+
+ m_A = pow(2.71828,(-1.0/(yaml_control_frequency*alpha*T)));
+ m_B = -alpha*T*(m_A-1.0);
+ m_C = k/(alpha*T)*(1.0-1.0/alpha);
+ m_D = k/alpha;
+
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Parameters changed to k="<<k<<", T="<<T<<", alpha="<<alpha);
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Matrices changed to A="<<m_A<<", B="<<m_B<<", C="<<m_C<<", D="<<m_D);
+
+
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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("[CSONE 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("[CSONE 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("[CSONE 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("[CSONE 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
+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 = "CsoneController";
+ // 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("[CSONE CONTROLLER] The request load yaml file service call failed.");
+ }
+}
+
+
+// CALLBACK NOTIFYING THAT THE YAML PARAMETERS ARE READY TO BE LOADED
+void isReadyCsoneControllerYamlCallback(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("[CSONE CONTROLLER] Now fetching the CsoneController 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("[CSONE CONTROLLER] Now fetching the CsoneController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[CSONE 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
+ fetchCsoneControllerYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+// LOADING OF THE YAML PARAMTERS
+void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // CsoneController.yaml
+
+ // Add the "CsoneController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "CsoneController");
+
+
+
+ // 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);
+
+ // 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");
+
+
+ // > DEBUGGING: Print out one of the parameters that was loaded to
+ // debug if the fetching of parameters worked correctly
+ ROS_INFO_STREAM("[CSONE CONTROLLER] DEBUGGING: the fetched CsoneController/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("[CSONE 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, "CsoneControllerService");
+
+ // 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 "CsoneControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CSONE 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("[CSONE CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[CSONE 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("[CSONE CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[CSONE 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( "CsoneController", 1, isReadyCsoneControllerYamlCallback);
+ ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("CsoneController", 1, isReadyCsoneControllerYamlCallback);
+
+
+
+ // 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 service 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(2.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("CsoneControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // 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 requestControllerParametersChangeSubscriber = nodeHandle.subscribe("RequestControllerParametersChange", 1, requestControllerParametersChangeCallback);
+
+ // 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:
+ // variable name: "requestIntegatorStateChangeSubscriber"
+ // variable type: ros::Subscriber
+ // name of message: "RequestIntegratorStateChange"
+ // The message has the structure defined in the file:
+ // "IntWithHeader.msg" (located in the "msg" folder).
+ // The messages received by this subscriber typically come from
+ // the "flying agent GUI".
+ ros::Subscriber requestIntegatorStateChangeSubscriber = nodeHandle.subscribe("RequestIntegratorStateChange", 1, requestIntegratorStateChangeCallback);
+
+ // Instantiate the class variable:
+ // variable name: "m_integratorStateChangedPublisher"
+ // variable type: ros::Publisher
+ // name of message: "IntegratorStateChanged"
+ // The message has the structure defined in the file:
+ // "IntWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current state of the integator.
+ m_integratorStateChangedPublisher = nodeHandle.advertise<IntWithHeader>("IntegratorStateChanged", 1);
+
+ // Instantiate the local variable:
+ // variable name: "getCurrentIntegratorStateService"
+ // variable type: ros::ServiceServer
+ // name of service: "GetCurrentIntegratorState"
+ // callback funcion: "getCurrentIntegratorStateCallback"
+ // This service has the input-output behaviour defined in the file:
+ // "IntIntService.srv" (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
+ // state of the integrator.
+ ros::ServiceServer getCurrentIntegratorStateService = nodeHandle.advertiseService("GetCurrentIntegratorState", getCurrentIntegratorStateCallback);
+
+
+ // Instantiate the local variable:
+ // variable name: "requestTimeDelayChangeSubscriber"
+ // variable type: ros::Subscriber
+ // name of message: "RequestTimeDelayChange"
+ // The message has the structure defined in the file:
+ // "IntWithHeader.msg" (located in the "msg" folder).
+ // The messages received by this subscriber typically come from
+ // the "flying agent GUI".
+ ros::Subscriber requestTimeDelayChangeSubscriber = nodeHandle.subscribe("RequestTimeDelayChange", 1, requestTimeDelayChangeCallback);
+
+
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "CsoneController". 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("CsoneController", 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("CsoneControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
+
+
+
+ // Print out some information to the user.
+ ROS_INFO("[CSONE 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/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 05b1a91c07d4e27cefc3159423440284c360b1b6..3e6cd6d9bec1c49c03596b4d92bda2d4195cd8e3 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -188,7 +188,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
switch (m_current_state)
{
case DEFAULT_CONTROLLER_STATE_NORMAL:
- computeResponse_for_normal(response);
+ computeResponse_for_normal(request.isFirstControllerCall, response);
break;
case DEFAULT_CONTROLLER_STATE_TAKEOFF_SPIN_MOTORS:
@@ -295,10 +295,10 @@ void computeResponse_for_standby(Controller::Response &response)
}
-void computeResponse_for_normal(Controller::Response &response)
+void computeResponse_for_normal(bool isFirstControllerCall,Controller::Response &response)
{
// Check if the state "just recently" changed
- if (m_current_state_changed)
+ if (m_current_state_changed || isFirstControllerCall)
{
// PERFORM "ONE-OFF" OPERATIONS HERE
// Set the "m_setpoint_for_controller" variable
@@ -317,7 +317,7 @@ void computeResponse_for_normal(Controller::Response &response)
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);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
}
@@ -415,7 +415,7 @@ void computeResponse_for_takeoff_move_up(Controller::Response &response)
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);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
// Change to next state after specified time
if (m_time_in_seconds > yaml_takoff_move_up_time)
@@ -459,7 +459,7 @@ void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response)
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);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
// Check if within the "integrator on" box of the setpoint
// > First, compute the current errors
@@ -520,7 +520,7 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
// accordingly
static bool static_integrator_complete_once = false;
static float static_integrator_on_time = yaml_takoff_integrator_on_time;
- static int static_integrator_flag = INTEGRATOR_FLAG_ON;
+ static int static_integrator_flag = DEFAULT_INTEGRATOR_FLAG_ON;
// Check if the state "just recently" changed
if (m_current_state_changed)
@@ -535,12 +535,12 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
// Adjust the integrator on/off and time
if (static_integrator_complete_once)
{
- static_integrator_flag = INTEGRATOR_FLAG_OFF;
+ static_integrator_flag = DEFAULT_INTEGRATOR_FLAG_OFF;
static_integrator_on_time = 0.5;
}
else
{
- static_integrator_flag = INTEGRATOR_FLAG_ON;
+ static_integrator_flag = DEFAULT_INTEGRATOR_FLAG_ON;
static_integrator_on_time = yaml_takoff_integrator_on_time;
}
// Set the change flag back to false
@@ -605,7 +605,7 @@ void computeResponse_for_landing_move_down(Controller::Response &response)
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);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
// Check if within the threshold of zero
if (m_current_stateInertialEstimate[2] < yaml_landing_move_down_end_height_threshold)
@@ -657,7 +657,7 @@ void computeResponse_for_landing_spin_motors(Controller::Response &response)
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);
+ calculateControlOutput_viaLQR_givenSetpoint(m_setpoint_for_controller, m_current_stateInertialEstimate, response, DEFAULT_INTEGRATOR_FLAG_OFF);
// Compute the desired "spinning" thrust
float thrust_for_spinning =
(1.0-time_elapsed_proportion)
@@ -794,11 +794,11 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// RUN THE FINITE DIFFERENCE ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
@@ -853,7 +853,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate)
{
// PUT IN THE CURRENT MEASUREMENT DIRECTLY
// > for (x,y,z) position
@@ -866,15 +866,26 @@ void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
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;
+ // > Only if this is NOT the first call to the controller
+ if (!isFirstUpdate)
+ {
+ // > 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;
+ }
+ else
+ {
+ // Set the velocities to zero
+ m_stateInterialEstimate_viaFiniteDifference[3] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[4] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[5] = 0.0;
+ }
}
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate)
{
// PERFORM THE KALMAN FILTER UPDATE STEP
// > First take a copy of the estimator state
@@ -883,6 +894,18 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
{
temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
+ // If this is the "first update", then:
+ if (isFirstUpdate)
+ {
+ // Set the positions to the current measurement
+ temp_PMKFstate[0] = m_current_xzy_rpy_measurement[0];
+ temp_PMKFstate[1] = m_current_xzy_rpy_measurement[1];
+ temp_PMKFstate[2] = m_current_xzy_rpy_measurement[2];
+ // Set the velocities to zero
+ temp_PMKFstate[3] = 0.0;
+ temp_PMKFstate[4] = 0.0;
+ temp_PMKFstate[5] = 0.0;
+ }
// > 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];
@@ -1084,14 +1107,14 @@ void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controlle
// PERFORM THE INTEGRATOR COMPUTATIONS
- if (integrator_flag == INTEGRATOR_FLAG_ON)
+ if (integrator_flag == DEFAULT_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)
+ else if (integrator_flag == DEFAULT_INTEGRATOR_FLAG_RESET)
{
newtons_int = 0.0;
tau_x = 0.0;
@@ -1358,6 +1381,8 @@ void setNewSetpoint(float x, float y, float z, float yaw)
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);
}
@@ -1459,7 +1484,7 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// Call the controller function with the reset flag
float tempStateBodyError[9];
Controller::Response temp_response;
- calculateControlOutput_viaLQR_givenError(tempStateBodyError, temp_response, INTEGRATOR_FLAG_RESET);
+ calculateControlOutput_viaLQR_givenError(tempStateBodyError, temp_response, DEFAULT_INTEGRATOR_FLAG_RESET);
break;
}
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 1f36a2cb7fa3f262c25e285daea14f8ded4fbf05..63445d1be767f07bddde21d22fa206d891c92277 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -190,6 +190,17 @@ void viconCallback(const ViconData& viconData)
{
controllerCall.request.otherCrazyflies.push_back(poseDataForOtherAgent);
}
+
+ // Fill in the "isFirstControllerCall" field
+ if (m_isFirstCall_to_instant_controller)
+ {
+ controllerCall.request.isFirstControllerCall = true;
+ m_isFirstCall_to_instant_controller = false;
+ }
+ else
+ {
+ controllerCall.request.isFirstControllerCall = false;
+ }
// PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
@@ -220,6 +231,9 @@ void viconCallback(const ViconData& viconData)
{
// Set the DEFAULT controller to be active
setInstantController(DEFAULT_CONTROLLER);
+ // Update the "isFirstControllerCall" field
+ controllerCall.request.isFirstControllerCall = true;
+ m_isFirstCall_to_instant_controller = false;
// Try the controller call
isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
// Inform the user is not successful
@@ -823,6 +837,11 @@ void setInstantController(int controller)
// Update the class variable
m_instant_controller = controller;
+ // Set the class variable indicating that the next
+ // controller call should be flagged as the first
+ // call for this instant controller
+ m_isFirstCall_to_instant_controller = true;
+
// Point to the correct service client
switch(controller)
{
@@ -844,6 +863,9 @@ void setInstantController(int controller)
case TEMPLATE_CONTROLLER:
m_instant_controller_service_client = &m_templateController;
break;
+ case CSONE_CONTROLLER:
+ m_instant_controller_service_client = &m_csoneController;
+ break;
case TESTMOTORS_CONTROLLER:
m_instant_controller_service_client = &m_defaultController;
break;
@@ -984,6 +1006,11 @@ void flyingStateRequestCallback(const IntWithHeader & msg) {
setControllerNominallySelected(TEMPLATE_CONTROLLER);
break;
+ case CMD_USE_CSONE_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_CSONE_CONTROLLER Command received");
+ setControllerNominallySelected(CSONE_CONTROLLER);
+ break;
+
case CMD_CRAZYFLY_TAKE_OFF:
ROS_INFO("[FLYING AGENT CLIENT] TAKE_OFF Command received");
@@ -1277,6 +1304,7 @@ void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
createControllerServiceClientFromParameterName( "pickerController" , m_pickerController );
createControllerServiceClientFromParameterName( "remoteController" , m_remoteController );
createControllerServiceClientFromParameterName( "templateController" , m_templateController );
+ createControllerServiceClientFromParameterName( "csoneController" , m_csoneController );
createControllerServiceClientFromParameterName( "testMotorsController" , m_testMotorsController );
@@ -1588,7 +1616,7 @@ int main(int argc, char* argv[])
// EMERGENCY STOP OF THE WHOLE "D-FaLL-System"
- ros::Subscriber emergencyStopSubscriber = nodeHandle_dfall_root.subscribe("emergencyStop", 1, emergencyStopReceivedCallback);
+ ros::Subscriber emergencyStopSubscriber = nodeHandle_dfall_root.subscribe("EmergencyStop", 1, emergencyStopReceivedCallback);
// LOCALISATION DATA FROM MOTION CAPTURE SYSTEM
@@ -1615,20 +1643,20 @@ int main(int argc, char* argv[])
// 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);
+ 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);
+ 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);
+ m_controllerUsedPublisher = nodeHandle.advertise<std_msgs::Int32>("ControllerUsed", 1);
// SUBSCRIBER FOR CRAZY RADIO STATUS CHANGES
@@ -1658,13 +1686,13 @@ int main(int argc, char* argv[])
// 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);
+ 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);
+ ros::ServiceServer getInstantControllerService = nodeHandle.advertiseService("GetInstantController", getInstantControllerServiceCallback);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
index f1b3c85ad8efe4b0b0c938f793d4a88d5e708e30..b30281bf67f157480077faf37d962938db4e5939 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
@@ -458,11 +458,11 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// RUN THE FINITE DIFFERENCE ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
@@ -517,7 +517,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
-void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate)
{
// PUT IN THE CURRENT MEASUREMENT DIRECTLY
// > for (x,y,z) position
@@ -530,19 +530,33 @@ void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
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;
+ // > Only if this is NOT the first call to the controller
+ if (!isFirstUpdate)
+ {
+ // > 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;
+ }
+ else
+ {
+ // Set the velocities to zero
+ m_stateInterialEstimate_viaFiniteDifference[3] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[4] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[5] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[9] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[10] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[11] = 0.0;
+ }
}
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate)
{
// PERFORM THE KALMAN FILTER UPDATE STEP
// > First take a copy of the estimator state
@@ -551,6 +565,26 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
{
temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
+ // If this is the "first update", then:
+ if (isFirstUpdate)
+ {
+ // Set the positions to the current measurement
+ temp_PMKFstate[0] = m_current_xzy_rpy_measurement[0];
+ temp_PMKFstate[1] = m_current_xzy_rpy_measurement[1];
+ temp_PMKFstate[2] = m_current_xzy_rpy_measurement[2];
+ // Set the velocities to zero
+ temp_PMKFstate[3] = 0.0;
+ temp_PMKFstate[4] = 0.0;
+ temp_PMKFstate[5] = 0.0;
+ // Set the angles to the current measurement
+ temp_PMKFstate[6] = m_current_xzy_rpy_measurement[3];
+ temp_PMKFstate[7] = m_current_xzy_rpy_measurement[4];
+ temp_PMKFstate[8] = m_current_xzy_rpy_measurement[5];
+ // Set the velocities to zero
+ temp_PMKFstate[9] = 0.0;
+ temp_PMKFstate[10] = 0.0;
+ temp_PMKFstate[11] = 0.0;
+ }
// > 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];
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
index a2887e62cbb1eb6025d3a9854f5360179c2b4c87..e1e01f283eeb80461d021d89b2f19b111cb771af 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
@@ -174,7 +174,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
// > After this function is complete the class variable
- // "current_stateInertialEstimate" is updated and ready
+ // "m_current_stateInertialEstimate" is updated and ready
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
@@ -184,7 +184,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if (isActive_remoteControlMode)
{
// CARRY OUT THE CONTROLLER COMPUTATIONS
- calculateControlOutput_forRemoteControl(current_stateInertialEstimate,request,response);
+ calculateControlOutput_forRemoteControl(m_current_stateInertialEstimate,request,response);
}
else
@@ -194,7 +194,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > Define a local array to fill in with the body frame error
float current_bodyFrameError[12];
// > Call the function to perform the conversion
- convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+ convert_stateInertial_to_bodyFrameError(m_current_stateInertialEstimate,setpoint,current_bodyFrameError);
@@ -481,25 +481,25 @@ 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;
+ 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
- current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
- current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
- current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+ 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();
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
- switch (estimator_method)
+ switch (yaml_estimator_method)
{
// Estimator based on finte differences
case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
@@ -507,7 +507,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -517,19 +517,19 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
break;
}
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
- ROS_INFO_STREAM("[REMOTE CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'RemoteControllerService': the 'estimator_method' is not recognised.");
+ // Display that the "yaml_estimator_method" was not recognised
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'RemoteControllerService': the 'yaml_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];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -539,71 +539,105 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// 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];
+ 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
- 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];
+ 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()
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate)
{
// 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];
+ 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
- stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
- stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
- stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+ 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
- 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;
+ // > Only if this is NOT the first call to the controller
+ if (!isFirstUpdate)
+ {
+ // > 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;
+ }
+ else
+ {
+ // Set the velocities to zero
+ m_stateInterialEstimate_viaFiniteDifference[3] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[4] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[5] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[9] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[10] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[11] = 0.0;
+ }
}
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate)
{
// 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];
+ temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // If this is the "first update", then:
+ if (isFirstUpdate)
+ {
+ // Set the positions to the current measurement
+ temp_PMKFstate[0] = m_current_xzy_rpy_measurement[0];
+ temp_PMKFstate[1] = m_current_xzy_rpy_measurement[1];
+ temp_PMKFstate[2] = m_current_xzy_rpy_measurement[2];
+ // Set the velocities to zero
+ temp_PMKFstate[3] = 0.0;
+ temp_PMKFstate[4] = 0.0;
+ temp_PMKFstate[5] = 0.0;
+ // Set the angles to the current measurement
+ temp_PMKFstate[6] = m_current_xzy_rpy_measurement[3];
+ temp_PMKFstate[7] = m_current_xzy_rpy_measurement[4];
+ temp_PMKFstate[8] = m_current_xzy_rpy_measurement[5];
+ // Set the velocities to zero
+ temp_PMKFstate[9] = 0.0;
+ temp_PMKFstate[10] = 0.0;
+ temp_PMKFstate[11] = 0.0;
}
// > 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];
+ m_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]*m_current_xzy_rpy_measurement[0];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[1];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[2];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[3];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[4];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[5];
+ m_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]*m_current_xzy_rpy_measurement[5];
}
@@ -1361,7 +1395,7 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > 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_paramaters, "vicon_frequency");
+ yaml_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
@@ -1387,7 +1421,7 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
controller_mode = getParameterInt( nodeHandle_for_paramaters , "controller_mode" );
// A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
+ yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
@@ -1438,7 +1472,7 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
gravity_force_quarter = cf_mass * 9.81/(1000.0*4.0);
// Set that the estimator frequency is the same as the control frequency
- estimator_frequency = vicon_frequency;
+ m_estimator_frequency = yaml_vicon_frequency;
// Roll and pitch limit (in degrees for angles)
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
index 5a901340b55b623911c3a332197fdd3374b421a8..4266a77b9b69f81534e15062e2555bf9d534f4b2 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -188,10 +188,21 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
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;
+ // > But only if this is NOT the first call to the controller
+ if (!request.isFirstControllerCall)
+ {
+ // > Compute 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;
+ }
+ else
+ {
+ // Set the velocities to zero
+ stateErrorInertial[3] = 0.0;
+ stateErrorInertial[4] = 0.0;
+ stateErrorInertial[5] = 0.0;
+ }
// Fill in the roll and pitch angle measurements directly
stateErrorInertial[6] = request.ownCrazyflie.roll;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
index 99010a9998040cf4e74a137c11fb291df4ad1c49..535d907c1d650d29a80ffa3b49beb4acc32be1f6 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
@@ -191,11 +191,22 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
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;
-
+ // > But only if this is NOT the first call to the controller
+ if (!request.isFirstControllerCall)
+ {
+ // > Compute 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;
+ }
+ else
+ {
+ // Set the velocities to zero
+ stateErrorInertial[3] = 0.0;
+ stateErrorInertial[4] = 0.0;
+ stateErrorInertial[5] = 0.0;
+ }
+
// Fill in the roll and pitch angle measurements directly
stateErrorInertial[6] = request.ownCrazyflie.roll;
stateErrorInertial[7] = request.ownCrazyflie.pitch;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
index f02aad1614bf949a2a98d3196e8be4f722b17908..d18bd7ee6ad97304e497a71f9699f2536ca41c7a 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
@@ -174,7 +174,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
// > After this function is complete the class variable
- // "current_stateInertialEstimate" is updated and ready
+ // "m_current_stateInertialEstimate" is updated and ready
// to be used for subsequent controller copmutations
performEstimatorUpdate_forStateInterial(request);
@@ -184,7 +184,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > Define a local array to fill in with the body frame error
float current_bodyFrameError[12];
// > Call the function to perform the conversion
- convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+ convert_stateInertial_to_bodyFrameError(m_current_stateInertialEstimate,setpoint,current_bodyFrameError);
// UPDATE THE SETPOINT FOR FLYING IN A CIRCLE - IF ACTIVE
@@ -273,7 +273,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_horizontal_currentpoint" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_horizontal_currentpoint' is not recognised.");
break;
}
@@ -319,7 +319,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_vertical_currentpoint" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_vertical_currentpoint' is not recognised.");
break;
}
@@ -365,7 +365,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_heading_currentpoint" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_heading_currentpoint' is not recognised.");
break;
}
@@ -411,7 +411,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_all_currentpoint" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_all_currentpoint' is not recognised.");
break;
}
@@ -442,7 +442,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
+ // Display that the "test_index" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_index' is not recognised.");
break;
}
@@ -453,7 +453,7 @@ void activateTestCallback(const std_msgs::Int32& msg)
void update_setpoint_for_test_circle()
{
// Compute the time since start
- float time_since_start = float(test_circle_ticks_since_start) / vicon_frequency;
+ float time_since_start = float(test_circle_ticks_since_start) / yaml_vicon_frequency;
// Allow time to reach the start point
if (time_since_start < test_circle_time_to_reach_start)
@@ -550,25 +550,25 @@ 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;
+ 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
- current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
- current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
- current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+ 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();
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
// RUN THE POINT MASS KALMAN FILTER ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
// FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
- switch (estimator_method)
+ switch (yaml_estimator_method)
{
// Estimator based on finte differences
case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
@@ -576,7 +576,7 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -586,19 +586,19 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// Transfer the estimate
for(int i = 0; i < 12; ++i)
{
- current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
}
break;
}
// Handle the exception
default:
{
- // Display that the "estimator_method" was not recognised
- ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'TuningControllerService': the 'estimator_method' is not recognised.");
+ // Display that the "yaml_estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'TuningControllerService': the 'yaml_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];
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
}
break;
}
@@ -608,71 +608,105 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
// 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];
+ 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
- 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];
+ 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()
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpdate)
{
// 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];
+ 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
- stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
- stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
- stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+ 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
- 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;
+ // > Only if this is NOT the first call to the controller
+ if (!isFirstUpdate)
+ {
+ // > 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;
+ }
+ else
+ {
+ // Set the velocities to zero
+ m_stateInterialEstimate_viaFiniteDifference[3] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[4] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[5] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[9] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[10] = 0.0;
+ m_stateInterialEstimate_viaFiniteDifference[11] = 0.0;
+ }
}
-void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(bool isFirstUpdate)
{
// 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];
+ temp_PMKFstate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // If this is the "first update", then:
+ if (isFirstUpdate)
+ {
+ // Set the positions to the current measurement
+ temp_PMKFstate[0] = m_current_xzy_rpy_measurement[0];
+ temp_PMKFstate[1] = m_current_xzy_rpy_measurement[1];
+ temp_PMKFstate[2] = m_current_xzy_rpy_measurement[2];
+ // Set the velocities to zero
+ temp_PMKFstate[3] = 0.0;
+ temp_PMKFstate[4] = 0.0;
+ temp_PMKFstate[5] = 0.0;
+ // Set the angles to the current measurement
+ temp_PMKFstate[6] = m_current_xzy_rpy_measurement[3];
+ temp_PMKFstate[7] = m_current_xzy_rpy_measurement[4];
+ temp_PMKFstate[8] = m_current_xzy_rpy_measurement[5];
+ // Set the velocities to zero
+ temp_PMKFstate[9] = 0.0;
+ temp_PMKFstate[10] = 0.0;
+ temp_PMKFstate[11] = 0.0;
}
// > 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];
+ m_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]*m_current_xzy_rpy_measurement[0];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[1];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[2];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[3];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[4];
+ m_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]*m_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];
+ m_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]*m_current_xzy_rpy_measurement[5];
+ m_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]*m_current_xzy_rpy_measurement[5];
}
@@ -1494,7 +1528,7 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > 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_paramaters, "vicon_frequency");
+ yaml_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
@@ -1520,7 +1554,7 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
controller_mode = getParameterInt( nodeHandle_for_paramaters , "controller_mode" );
// A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
+ yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
@@ -1573,7 +1607,7 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
gravity_force_quarter = cf_mass * 9.81/(1000*4);
// Set that the estimator frequency is the same as the control frequency
- estimator_frequency = vicon_frequency;
+ m_estimator_frequency = yaml_vicon_frequency;
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: after processing the gravity_force_quarter = " << gravity_force_quarter);
diff --git a/dfall_ws/src/dfall_pkg/srv/Controller.srv b/dfall_ws/src/dfall_pkg/srv/Controller.srv
index ff3dead3b29c12a4f325a86729d99ff6bac7adf6..979abc5c4bd047133bd520771cb712443331c16e 100644
--- a/dfall_ws/src/dfall_pkg/srv/Controller.srv
+++ b/dfall_ws/src/dfall_pkg/srv/Controller.srv
@@ -1,3 +1,4 @@
+bool isFirstControllerCall
CrazyflieData ownCrazyflie
CrazyflieData[] otherCrazyflies
---
diff --git a/install/dfall_install_ros_kinetic_ubuntu16.sh b/install/dfall_install_ros_kinetic_ubuntu16.sh
index ecc29b27d6d6c3eb171583dc233414b231105eeb..ae1f9471b7c8a966a6d3d30d81c54a404a943301 100755
--- a/install/dfall_install_ros_kinetic_ubuntu16.sh
+++ b/install/dfall_install_ros_kinetic_ubuntu16.sh
@@ -88,6 +88,13 @@ sudo apt -y install qt5-default
# Install the development version of the Qt5svg library
sudo apt -y install libqt5svg5-dev
+# Install the charting library
+sudo apt install libqt5charts5-dev
+
+# Install the multimedia library (used for sound effects)
+sudo apt install libqt5multimedia5
+sudo apt install qtmultimedia5-dev
+
# Install the "Qt Creator" IDE
# > NOTE: this is not necessary to compile and run the dfall ROS package,
# but it is required to edit the GUIs
diff --git a/install/dfall_install_ros_melodic_ubuntu18.sh b/install/dfall_install_ros_melodic_ubuntu18.sh
index abcfc1df324d9a27f7d4a1f88f735e281abf5f65..982da3cea59976ae1583a88d65ecacf873d866d7 100755
--- a/install/dfall_install_ros_melodic_ubuntu18.sh
+++ b/install/dfall_install_ros_melodic_ubuntu18.sh
@@ -88,6 +88,13 @@ sudo apt -y install qt5-default
# Install the development version of the Qt5svg library
sudo apt -y install libqt5svg5-dev
+# Install the charting library
+sudo apt install libqt5charts5-dev
+
+# Install the multimedia library (used for sound effects)
+sudo apt install libqt5multimedia5
+sudo apt install qtmultimedia5-dev
+
# Install the "Qt Creator" IDE
# > NOTE: this is not necessary to compile and run the dfall ROS package,
# but it is required to edit the GUIs