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 64921ccc6888b776866b2135a6698a36a3c18cd9..c8225eaff2d6b6b9e9c3fc2b5d71237bce7b629c 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
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1633</width>
- <height>765</height>
+ <width>1614</width>
+ <height>991</height>
</rect>
</property>
<property name="font">
@@ -19,163 +19,79 @@
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout">
- <item row="3" column="13">
- <spacer name="horizontalSpacer">
+ <item row="1" column="0">
+ <widget class="Line" name="line">
<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="6" column="5">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="5" column="3">
- <widget class="QLabel" name="label_row_z">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>z [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="5" column="6">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <widget class="QLineEdit" name="lineEdit_error_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
</widget>
</item>
- <item row="0" column="4">
- <widget class="QLabel" name="label_error_title">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <property name="leftMargin">
+ <number>0</number>
</property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
+ <property name="topMargin">
+ <number>0</number>
</property>
- <property name="text">
- <string>Error</string>
+ <property name="rightMargin">
+ <number>0</number>
</property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <property name="bottomMargin">
+ <number>0</number>
</property>
- </widget>
+ <item>
+ <widget class="QLabel" name="label_current_state_title">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Current State:</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_current_state">
+ <property name="text">
+ <string>TextLabel</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</item>
- <item row="3" column="6">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
+ <item row="4" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
</property>
- <property name="maximumSize">
+ <property name="sizeHint" stdset="0">
<size>
- <width>180</width>
- <height>60</height>
+ <width>20</width>
+ <height>40</height>
</size>
</property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
+ </spacer>
</item>
- <item row="3" column="7">
- <spacer name="horizontalSpacer_2">
+ <item row="2" column="0">
+ <spacer name="verticalSpacer_2">
<property name="orientation">
- <enum>Qt::Horizontal</enum>
+ <enum>Qt::Vertical</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
@@ -188,247 +104,355 @@
</property>
</spacer>
</item>
- <item row="5" column="10">
- <layout class="QHBoxLayout" name="horizontalLayout_2">
- <item>
- <widget class="QPushButton" name="z_increment_minus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item row="2" column="1">
+ <widget class="QLabel" name="label_row_x">
<property name="maximumSize">
<size>
- <width>60</width>
+ <width>16777215</width>
<height>60</height>
</size>
</property>
<property name="text">
- <string>-</string>
+ <string>x [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
+ <item row="0" column="6">
+ <widget class="QLabel" name="label_increment_title">
<property name="maximumSize">
<size>
- <width>140</width>
- <height>60</height>
+ <width>16777215</width>
+ <height>50</height>
</size>
</property>
<property name="font">
<font>
- <family>Courier</family>
+ <weight>75</weight>
+ <bold>true</bold>
</font>
</property>
+ <property name="text">
+ <string>Increment</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
</widget>
</item>
- <item>
- <widget class="QPushButton" name="z_increment_plus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_error_title">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Error</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLabel" name="label_row_yaw">
<property name="maximumSize">
<size>
- <width>60</width>
+ <width>16777215</width>
<height>60</height>
</size>
</property>
<property name="text">
- <string>+</string>
+ <string>yaw [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- </layout>
- </item>
- <item row="4" column="3">
- <widget class="QLabel" name="label_row_y">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>y [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="3">
- <widget class="QLabel" name="label_row_x">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>x [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="4" column="10">
- <layout class="QHBoxLayout" name="horizontalLayout_3">
- <item>
- <widget class="QPushButton" name="y_increment_minus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_error_title_line2">
<property name="maximumSize">
<size>
- <width>60</width>
- <height>60</height>
+ <width>16777215</width>
+ <height>50</height>
</size>
</property>
<property name="text">
- <string>-</string>
+ <string>meas-ref</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
+ <item row="1" column="3">
+ <widget class="QLabel" name="label_current_title_line2">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLabel" name="label_current_title">
<property name="maximumSize">
<size>
- <width>140</width>
- <height>60</height>
+ <width>16777215</width>
+ <height>50</height>
</size>
</property>
<property name="font">
<font>
- <family>Courier</family>
+ <weight>75</weight>
+ <bold>true</bold>
</font>
</property>
+ <property name="text">
+ <string>Current</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
</widget>
</item>
- <item>
- <widget class="QPushButton" name="y_increment_plus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
+ <item row="1" column="6">
+ <widget class="QLabel" name="label_increment_title_line2">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="label_row_y">
<property name="maximumSize">
<size>
- <width>60</width>
+ <width>16777215</width>
<height>60</height>
</size>
</property>
<property name="text">
- <string>+</string>
+ <string>y [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- </layout>
- </item>
- <item row="4" column="6">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="4" column="4">
- <widget class="QLineEdit" name="lineEdit_error_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="6" column="10">
- <layout class="QHBoxLayout" name="horizontalLayout">
- <item>
- <widget class="QPushButton" name="yaw_increment_minus_button">
+ <item row="3" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item>
+ <widget class="QPushButton" name="y_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="y_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QPushButton" name="z_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="z_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>60</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
<property name="text">
- <string>-</string>
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_yaw">
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_yaw">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>140</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
@@ -437,282 +461,183 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
</widget>
</item>
- <item>
- <widget class="QPushButton" name="yaw_increment_plus_button">
+ <item row="5" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QPushButton" name="yaw_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="yaw_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_yaw">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>60</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
<property name="text">
- <string>+</string>
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- </layout>
- </item>
- <item row="0" column="10">
- <widget class="QLabel" name="label_increment_title">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Increment</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="6">
- <widget class="QLabel" name="label_new_title_line2">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Setpoint</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="4" column="5">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="5">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="2" column="5">
- <widget class="QLabel" name="label_current_title_line2">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Setpoint</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="5" column="5">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="5" column="4">
- <widget class="QLineEdit" name="lineEdit_error_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="6" column="4">
- <widget class="QLineEdit" name="lineEdit_error_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="6" column="3">
- <widget class="QLabel" name="label_row_yaw">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>yaw [deg]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="4">
- <widget class="QLabel" name="label_error_title_line2">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>meas-ref</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="10">
- <layout class="QHBoxLayout" name="horizontalLayout_4">
- <item>
- <widget class="QPushButton" name="x_increment_minus_button">
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_x">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>60</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
<property name="text">
- <string>-</string>
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_x">
+ <item row="7" column="1">
+ <widget class="QLabel" name="label_row_roll">
+ <property name="text">
+ <string>roll [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="3">
+ <widget class="QPushButton" name="default_setpoint_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>140</width>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
<height>60</height>
</size>
</property>
@@ -721,415 +646,668 @@
<family>Courier</family>
</font>
</property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_measured_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Measured</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
</widget>
</item>
- <item>
- <widget class="QPushButton" name="x_increment_plus_button">
+ <item row="6" column="1">
+ <widget class="QLabel" name="label_row_pitch">
+ <property name="text">
+ <string>pitch [deg]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_y">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>60</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
<property name="text">
- <string>+</string>
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- </layout>
- </item>
- <item row="0" column="5">
- <widget class="QLabel" name="label_current_title">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Current</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="2" column="10">
- <widget class="QLabel" name="label_increment_title_line2">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Setpoint</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="6" column="6">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- <item row="0" column="6">
- <widget class="QLabel" name="label_new_title">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>New</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="7" column="5">
- <widget class="QPushButton" name="default_setpoint_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Default</string>
- </property>
- </widget>
- </item>
- <item row="7" column="6">
- <widget class="QPushButton" name="set_setpoint_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Set New</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="lineEdit_measured_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_measured_title">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Measured</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="7" column="3">
- <widget class="QLabel" name="label_row_pitch">
- <property name="text">
- <string>pitch [deg]</string>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="lineEdit_measured_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QLineEdit" name="lineEdit_measured_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="8" column="3">
- <widget class="QLabel" name="label_row_roll">
- <property name="text">
- <string>roll [deg]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="6" column="1">
- <widget class="QLineEdit" name="lineEdit_measured_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="7" column="1">
- <widget class="QLineEdit" name="lineEdit_measured_pitch">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="8" column="1">
- <widget class="QLineEdit" name="lineEdit_measured_roll">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- </widget>
- </item>
- <item row="10" column="1">
- <spacer name="verticalSpacer">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="2" column="1">
- <layout class="QHBoxLayout" name="horizontalLayout_5">
- <property name="spacing">
- <number>0</number>
- </property>
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item>
- <widget class="QFrame" name="red_frame_position_left">
+ <item row="4" column="1">
+ <widget class="QLabel" name="label_row_z">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
<property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>10</width>
- <height>16777215</height>
+ <width>180</width>
+ <height>60</height>
</size>
</property>
- <property name="styleSheet">
- <string notr="true">background-color:red;</string>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
</property>
- <property name="frameShape">
- <enum>QFrame::StyledPanel</enum>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
- <property name="frameShadow">
- <enum>QFrame::Raised</enum>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item>
- <widget class="QLabel" name="label_measured_title_line2">
+ <item row="4" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
<property name="text">
- <string>Position</string>
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <item>
+ <widget class="QPushButton" name="x_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>-</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>140</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="x_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>60</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>+</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_5">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QFrame" name="red_frame_position_left">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_measured_title_line2">
+ <property name="text">
+ <string>Position</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="red_frame_position_right">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_12">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="7" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_13">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="7">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="2" column="5">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="4">
+ <widget class="QLabel" name="label_new_title">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>New</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item>
- <widget class="QFrame" name="red_frame_position_right">
+ <item row="1" column="4">
+ <widget class="QLabel" name="label_new_title_line2">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
<property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>10</width>
- <height>16777215</height>
+ <width>180</width>
+ <height>60</height>
</size>
</property>
- <property name="styleSheet">
- <string notr="true">background-color:red;</string>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
- <property name="frameShape">
- <enum>QFrame::StyledPanel</enum>
+ </widget>
+ </item>
+ <item row="5" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
- <property name="frameShadow">
- <enum>QFrame::Raised</enum>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="4">
+ <widget class="QPushButton" name="set_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set New</string>
</property>
</widget>
</item>
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 c1df7fef3ff18ce65ae38879c1396e660eb7383f..e3e70de43498c3add8cd56bf6c419f1759bf1a69 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
@@ -492,66 +492,6 @@
</property>
</spacer>
</item>
- <item row="3" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
<item row="4" column="1">
<widget class="QLabel" name="label_row_y">
<property name="maximumSize">
@@ -568,126 +508,6 @@
</property>
</widget>
</item>
- <item row="7" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_pitch">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="8" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_roll">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="5" column="0">
- <widget class="QLineEdit" name="lineEdit_measured_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
<item row="7" column="1">
<widget class="QLabel" name="label_row_pitch">
<property name="maximumSize">
@@ -1379,6 +1199,210 @@
</property>
</spacer>
</item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="7" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="8" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
</layout>
</item>
<item row="14" column="0">
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
index c454832de21f0eb8a6f5ec3a1b2539fe4290fb84..148335ac623837db7a29677bb78e96019a34e8aa 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
@@ -60,6 +60,7 @@
// Include the shared definitions
#include "nodes/Constants.h"
+#include "nodes/DefaultControllerConstants.h"
// SPECIFY THE PACKAGE NAMESPACE
//using namespace dfall_pkg;
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 c527b64103b09f65ccc493098c10de34c8e06e4d..4dab1f6b053b85a34473baeec87121b115379822 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
@@ -47,6 +47,9 @@ DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
ui->red_frame_position_left->setVisible(false);
ui->red_frame_position_right->setVisible(false);
+ // Make the current state label blank
+ ui->label_current_state->setText("");
+
#ifdef CATKIN_MAKE
@@ -249,6 +252,52 @@ void DefaultControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWith
if (yaw < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 3));
+
+ // UPDATE THE CURRENT STATE LABEL
+ int current_state = newSetpoint.buttonID;
+ switch (current_state)
+ {
+ case DEFAULT_CONTROLLER_STATE_STANDBY:
+ {
+ ui->label_current_state->setText("standby");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS:
+ {
+ ui->label_current_state->setText("Take-off, spinning motors");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_TAKE_OFF_MOVE_UP:
+ {
+ ui->label_current_state->setText("Take-off, moving up");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_TAKE_OFF_GOTO_SETPOINT:
+ {
+ ui->label_current_state->setText("Take-off, goto setpoint");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN:
+ {
+ ui->label_current_state->setText("Landing, move down");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS:
+ {
+ ui->label_current_state->setText("Landing, spinning motors");
+ break;
+ }
+ case DEFAULT_CONTROLLER_STATE_UNKNOWN:
+ {
+ ui->label_current_state->setText("Unknown");
+ break;
+ }
+ default:
+ {
+ ui->label_current_state->setText("Not recognised");
+ break;
+ }
+ }
}
#endif
@@ -801,4 +850,4 @@ bool DefaultControllerTab::getTypeAndIDParameters()
// Return
return return_was_successful;
}
-#endif
\ No newline at end of file
+#endif
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf6d6cee985dda19defb0fc475f38cdb621070b2
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerConstants.h
@@ -0,0 +1,54 @@
+// 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:
+// Constants that are used across the Default Controler
+// Service, its GUI, and the Flying Agent Client
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+// TO REQUEST MANOEUVRES
+
+#define DEFAULT_CONTROLLER_REQUEST_TAKE_OFF 1
+#define DEFAULT_CONTROLLER_REQUEST_LANDING 2
+
+
+// TO IDENITFY THE STATE OF THE DEFAULT CONTROLLER
+
+#define DEFAULT_CONTROLLER_STATE_UNKNOWN -1
+#define DEFAULT_CONTROLLER_STATE_STANDBY 99
+
+// > The sequence of states for a TAKE-OFF manoeuvre
+#define DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS 11
+#define DEFAULT_CONTROLLER_STATE_TAKE_OFF_MOVE_UP 12
+#define DEFAULT_CONTROLLER_STATE_TAKE_OFF_GOTO_SETPOINT 13
+
+// > The sequence of states for a LANDING manoeuvre
+#define DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN 21
+#define DEFAULT_CONTROLLER_STATE_LANDING_SPIN_MOTORS 22
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index 00dc42196c9d777c3fc7c4864cbb100a63bee5e0..8da4fa3c57fbb27eee1b23ef86866179343458f6 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -72,6 +72,7 @@
// Include the shared definitions
#include "nodes/Constants.h"
+#include "nodes/DefaultControllerConstants.h"
// Include other classes
#include "classes/GetParamtersAndNamespaces.h"
@@ -114,6 +115,13 @@ using namespace dfall_pkg;
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
+// The current state of the Default Controller
+int m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
+
+// The elapased time, incremented by counting the motion
+// capture callbacks
+float m_time_in_seconds = 0.0;
+
// The ID of the agent that this node is monitoring
int m_agentID;
@@ -140,6 +148,7 @@ std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
// > the frequency at which the controller is running
float yaml_control_frequency = 200.0;
+float m_control_deltaT = (1.0/200.0);
// > the min and max for saturating 16 bit thrust commands
float yaml_command_sixteenbit_min = 1000;
@@ -172,8 +181,8 @@ std::vector<float> m_gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.
// ROS Publisher for debugging variables
ros::Publisher m_debugPublisher;
-// ROS Publisher for inform the network about
-// changes to the setpoin
+// ROS Publisher to inform the network about
+// changes to the setpoint
ros::Publisher m_setpointChangedPublisher;
@@ -201,6 +210,9 @@ ros::Publisher m_setpointChangedPublisher;
// need to written below in an order that ensure each function is
// implemented before it is called from another function)
+// CALLBACK FOR THE REQUEST MANOEUVRE SERVICE
+bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Response &response);
+
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
@@ -220,6 +232,9 @@ void setNewSetpoint(float x, float y, float z, float yaw);
// GET CURRENT SETPOINT SERVICE CALLBACK
bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+// PUBLISH THE CURRENT SETPOINT AND STATE
+void publishCurrentSetpointAndState();
+
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index 2d97b7c2f6c4896ca102acaf81ec7d76560f0983..863c8c7c530336224560bf33e88b662bcdbe8d35 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -66,6 +66,7 @@
// Include the shared definitions
#include "nodes/Constants.h"
+#include "nodes/DefaultControllerConstants.h"
// Include other classes
#include "classes/GetParamtersAndNamespaces.h"
@@ -121,74 +122,96 @@ std::string m_namespace_to_own_agent_parameter_service;
std::string m_namespace_to_coordinator_parameter_service;
-// variables for the states:
-int m_flying_state;
-bool m_changed_flying_state_flag;
-// variable for crazyradio status
-int crazyradio_status;
+// VARIABLES FOR THE MOTION CAPTURE DATA
+// > The index for which element in the "motion capture
+// data" array is expected to match the name in
+// "m_context", (negative numbers indicate unknown)
+int m_poseDataIndex = -1;
+// > Boolen indicating if the Mocap data is availble
+bool m_isAvailable_mocap_data = false;
+// > Boolen indicating if the object is "long term" occuled
+bool m_isOcculded_mocap_data = false;
+// > Number of consecutive occulsions that trigger the
+// "m_isOcculded_mocap_data" variable to be "true"
+int yaml_consecutive_occulsions_threshold = 10;
+// > Timer that when triggered determines that the
+// "m_isAvailable_mocap_data" variable becomes true
+ros::Timer m_timer_mocap_timeout_check;
+
+
+
+// VARIABLES FOR STORING THE PARAMTER OF THE POSITION
+// AND TILT ANGLE SAFTY CHECKS
+// > Boolean indicating whether the tilt angle check
+// should be performed
+bool yaml_isEnabled_strictSafety = true;
+// > The maximum allowed tilt angle, in degrees and radians
+float yaml_maxTiltAngle_for_strictSafety_degrees = 70;
+float m_maxTiltAngle_for_strictSafety_redians = 70 * DEG2RAD;
-//describes the area of the crazyflie and other parameters
-CrazyflieContext m_context;
-// The index for which element in the "motion captate data"
-// array is expected to match the name in "m_context"
-// > Negative numbers indicate unknown
-int m_poseDataIndex = -1;
-// wheter to use safe of demo controller
-// variable for the instant controller, e.g., we use
-// safe controller for taking off and landing even
-// if demo controller is enabled. This variable WILL change automatically
+// VARIABLES FOR MANAGING THE FLYING STATE
+// > Integer that is the current flying state
+int m_flying_state;
+// > Booleans for whether the {take-off,landing} should
+// be performed with the default controller
+bool yaml_shouldPerfom_takeOff_with_defaultController = true;
+bool yaml_shouldPerfom_takeOff_with_defaultController = true;
+// > Service Clients for requesting the Default controller
+// to perform a {take-off,landing} maneouvre
+ros::ServiceClient m_defaultController_requestManoeuvre;
+// > Timer that fire when the {take-off,landing} is complete
+ros::Timer m_timer_takeoff_complete;
+ros::Timer m_timer_land_complete;
+
+
+
+// VARIABLES RELATING TO CONTROLLER SELECTION
+// > Integer indicating the controller that is to be
+// used in when motion capture data is received
int m_instant_controller;
+// > Pointer to the controller service client that
+// agrees with the "m_instant_controller" variable
ros::ServiceClient* m_instant_controller_service_client;
+// > Boolean indicating that the controller service clients
+// have been successfully created
bool m_controllers_avialble = false;
+// > Timer for creating the controller service client after
+// some delay
ros::Timer timer_for_loadAllControllers;
-
-// controller used:
+// > Integer indicating the controller that has been
+// requested. This controller is used during the "flying"
+// state, and the "Default" controller is used during the
+// "take-off" and "landing" states.
int m_controller_nominally_selected;
-// The safe controller specified in the ClientConfig.yaml
-ros::ServiceClient safeController;
+
+// VARIABLES FOR THE CONTROLER SERVIVCE CLIENTS
// The default controller specified in the ClientConfig.yaml
-ros::ServiceClient defaultController;
-// The Demo controller specified in the ClientConfig.yaml
-ros::ServiceClient demoController;
+ros::ServiceClient m_defaultController;
// The Student controller specified in the ClientConfig.yaml
-ros::ServiceClient studentController;
-// The MPC controller specified in the ClientConfig.yaml
-ros::ServiceClient mpcController;
-// The Remote controller specified in the ClientConfig.yaml
-ros::ServiceClient remoteController;
+ros::ServiceClient m_studentController;
// The Tuning controller specified in the ClientConfig.yaml
-ros::ServiceClient tuningController;
+ros::ServiceClient m_tuningController;
// The Picker controller specified in the ClientConfig.yaml
-ros::ServiceClient pickerController;
+ros::ServiceClient m_pickerController;
// The Template controller specified in the ClientConfig.yaml
-ros::ServiceClient templateController;
+ros::ServiceClient m_templateController;
-// The values for the safety check on tilt angle
-bool yaml_isEnabled_strictSafety = true;
-float yaml_angleMargin;
-float yaml_maxTiltAngle_for_strictSafety_degrees = 70;
-float m_maxTiltAngle_for_strictSafety_redians = 70 * DEG2RAD;
-//Setpoint controller_setpoint;
-//std::vector<float> yaml_default_setpoint = {0.0f, 0.0f, 0.0f, 0.0f};
-
-// variables for linear trayectory
-//Setpoint current_safe_setpoint;
-//double distance;
-//double unit_vector[3];
-//bool was_in_threshold = false;
-//double yaml_distance_threshold; //to be loaded from yaml
+// variable for crazyradio status
+int m_crazyradio_status;
+//describes the area of the crazyflie and other parameters
+CrazyflieContext m_context;
// Service Client from which the "CrazyflieContext" is loaded
ros::ServiceClient centralManager;
@@ -199,9 +222,6 @@ ros::ServiceClient centralManager;
// {onboardControllerType,roll,pitch,yaw,motorCmd1,motorCmd2,motorCmd3,motorCmd4}
ros::Publisher commandForSendingToCrazyfliePublisher;
-// communicate with safeControllerService, setpoint, etc...
-//ros::Publisher safeControllerServiceSetpointPublisher;
-
// Publisher for the current flying state of this Flying Agent Client
ros::Publisher flyingStatePublisher;
@@ -212,27 +232,11 @@ ros::Publisher flyingStatePublisher;
// Publisher Communication with crazyRadio node. Connect and disconnect
ros::Publisher crazyRadioCommandPublisher;
-
// Publisher for which controller is currently being used
ros::Publisher controllerUsedPublisher;
-// float yaml_take_off_distance;
-// float yaml_landing_distance;
-// float yaml_duration_take_off;
-// float yaml_duration_landing;
-
-// bool finished_take_off = false;
-// bool finished_land = false;
-
-ros::Timer timer_takeoff;
-ros::Timer timer_land;
-
-// A ROS "bag" to store data for post-analysis
-//rosbag::Bag bag;
-
-
// ----------------------------------------------------------------------------------
@@ -251,75 +255,104 @@ ros::Timer timer_land;
-
-
-
-
-
-
+// FUNCTIONS FOR HANDLING THE MOTION CAPTURE DATA
+// > Callback run every time new motion capture
+// data is available
void viconCallback(const ViconData& viconData);
+// > For extracting the pose data of an specific
+// object by name
int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index , CrazyflieData& pose);
+// > For converting the global frame motion capture
+// data to the local frame of this agent
void coordinatesToLocal(CrazyflieData& cf);
+// > Callback run when motion capture data has not
+// been receive for a specified time
+void timerCallback_mocap_timeout_check(const ros::TimerEvent&);
+// > For sending a command, via the CrazyRadio, that
+// the motors should be set to zero
+void sendZeroOutputCommandForMotors();
-// > For the SAFETY CHECK on area and the angle
-bool safetyCheck(CrazyflieData data, ControlCommand controlCommand);
-
-
-void changeFlyingStateTo(int new_state);
-
-
-void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
-
-
-
+// FOR THE SAFETY CHECKS ON POSITION AND TILT ANGLE
+bool safetyCheck_on_positionAndTilt(CrazyflieData data);
-void flyingStateRequestCallback(const IntWithHeader & commandMsg);
+// FUNCTIONS THAT MANAGE THE CHANGES TO THE FLYING STATE
+// > For changing between possible state of:
+// {take-off,flying,take-off,motors-off}
+void requestChangeFlyingStateTo(int new_state);
+// > For changing to take-off
+void requestChangeFlyingStateToTakeOff();
+// > For changing to land
+void requestChangeFlyingStateToLand();
+// > Callback that the take off phase is complete
+void takeOffTimerCallback(const ros::TimerEvent&)
+// > Callback that the landing phase is complete
+void landTimerCallback(const ros::TimerEvent&)
-// void loadSafeController();
-// void loadDemoController();
-// void loadStudentController();
-// void loadMpcController();
-// void loadRemoteController();
-// void loadTuningController();
-// void loadPickerController();
-// void loadTemplateController();
-void loadController( std::string paramter_name , ros::ServiceClient& serviceClient );
-void sendMessageUsingController(int controller);
+// FUNCTIONS FOR SELECTING WHICH CONTROLLER TO USE
+// > For setting the controller that is actually used
void setInstantController(int controller);
+// > For retrieving the value of the class variable
int getInstantController();
+// > For setting the controller that it to be used
+// during the normal "flying" state
void setControllerNominallySelected(int controller);
+// > For retrieving the value of the class variable
int getControllerNominallySelected();
+// THE CALLBACK THAT A NEW FLYING STATE WAS REQUESTED
+// > These requests come from the "Flying Agent GUI"
+// > The options are: {take-off,land,motor-off,controller}
+void flyingStateOrControllerRequestCallback(const IntWithHeader & commandMsg);
+
+
+
// THE CALLBACK THAT THE CRAZY RADIO STATUS CHANGED
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
+
+
// THE CALLBACK THAT AN EMERGENCY STOP MESSAGE WAS RECEIVED
void emergencyStopReceivedCallback(const IntWithHeader & msg);
+
+
// THE SERVICE CALLBACK REQUESTING THE CURRENT FLYING STATE
bool getCurrentFlyingStateServiceCallback(IntIntService::Request &request, IntIntService::Response &response);
+
+
// FOR THE BATTERY STATE CALLBACK
void batteryMonitorStateChangedCallback(std_msgs::Int32 msg);
-// FOR THE SAFETY CHECKS ON POSITION AND TILT ANGLE
-bool safetyCheck(CrazyflieData data);
-// THE CALLBACK THAT THE CONTEXT DATABASE CHANGED
-void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
-// FOR LOADING THE CONTEXT OF THIS AGENT
+// FUNCTIONS FOR THE CONTEXT OF THIS AGENT
+// > Callback that the context database changed
+void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
+// > For loading the context of this agent
void loadCrazyflieContext();
+
+
+// FUNCTIONS FOR CREATING THE CONTROLLER SERVICE CLIENT
+// > For creating a service client from the name
+// of the YAML parameter
+void createControllerServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient );
+// > Callback for the timer so that all services servers
+// exists before we try to create the clients
+void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&);
+
+
+
// FOR LOADING THE YAML PARAMETERS
void isReadyClientConfigYamlCallback(const IntWithHeader & msg);
void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml b/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml
index 1d3c48063d12ab14fbaac949ecd3586d3d10561c..a5825598368247f9d1fd917af07971365b5aa3c6 100755
--- a/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml
@@ -1,17 +1,33 @@
-safeController: "SafeControllerService/RateController"
+# The name the controller services advertised by
+# each of the controller nodes.
defaultController: "DefaultControllerService/DefaultController"
-demoController: "DemoControllerService/DemoController"
studentController: "StudentControllerService/StudentController"
-mpcController: "MpcControllerService/MpcController"
-remoteController: "RemoteControllerService/RemoteController"
tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
templateController: "TemplateControllerService/TemplateController"
-# Flag for whether to use angle for switching to the Default Controller
+# The name of the service advertised by the Default
+# Controller for requests manoeuvres to be performed
+defaultController_requestManoeuvre: "DefaultControllerService/RequestManoeuvre"
+
+# Flag for whether to use angle for switching
+# to the Default Controller
isEnabled_strictSafety: true
maxTiltAngle_for_strictSafety_degrees: 70
-battery_threshold_while_flying: 2.60 # in V
-battery_threshold_while_motors_off: 3.30 # in V
-battery_polling_period: 200 # in ms
\ No newline at end of file
+# Number of consecutive occulsions that will deem the
+# object as "long-term" occuled
+consecutive_occulsions_threshold: 10
+
+# Flag for whether the take-off should be performed
+# with the default controller
+shouldPerfom_takeOff_with_defaultController: true
+
+# Flag for whether the landing should be performed
+# with the default controller
+shouldPerfom_landing_with_defaultController: true
+
+
+#battery_threshold_while_flying: 2.60 # in V
+#battery_threshold_while_motors_off: 3.30 # in V
+#battery_polling_period: 200 # in ms
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 7f455863252c53e61dc0b22906b06eca19ff3cd2..d6e0a5d8e1b8d336029cdad4892b946fee6454a3 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -59,6 +59,75 @@
+// ------------------------------------------------------------------------------
+// RRRR EEEEE QQQ U U EEEEE SSSS TTTTT
+// R R E Q Q U U E S T
+// RRRR EEE Q Q U U EEE SSS T
+// R R E Q Q U U E S T
+// R R EEEEE QQ Q UUU EEEEE SSSS T
+//
+// M M A N N OOO EEEEE U U V V RRRR EEEEE
+// MM MM A A NN N O O E U U V V R R E
+// M M M A A N N N O O EEE U U V V RRRR EEE
+// M M AAAAA N NN O O E U U V V R R E
+// M M A A N N OOO EEEEE UUU V R R EEEEE
+// ------------------------------------------------------------------------------
+
+// CALLBACK FOR THE REQUEST MANOEUVRE SERVICE
+bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Response &response)
+{
+ // Extract the requested manoeuvre
+ int requestedManoeuvre = request.data;
+
+ // Switch between the possible manoeuvres
+ switch (requestedManoeuvre)
+ {
+ case DEFAULT_CONTROLLER_REQUEST_TAKE_OFF:
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Received request to perform take-off manoeuvre.");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS;
+ // Fill in the response duration in milliseconds
+ response.data = 3000;
+ break;
+ }
+
+ case DEFAULT_CONTROLLER_REQUEST_LANDING:
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] Received request to perform landing manoeuvre.");
+ // Reset the time variable
+ m_time_in_seconds = 0.0;
+ // Update the state accordingly
+ m_current_state = DEFAULT_CONTROLLER_STATE_LANDING_MOVE_DOWN;
+ // Fill in the response duration in milliseconds
+ response.data = 2000;
+ break;
+ }
+
+ default:
+ {
+ // Inform the user
+ ROS_INFO("[DEFAULT CONTROLLER] The requested manoeuvre is not recognised. Hence switching to stand-by state.");
+ // Update the state to standby
+ m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
+ // Fill in the response duration in milliseconds
+ response.data = 0;
+ break;
+ }
+ }
+
+ // Publish the change
+ publishCurrentSetpointAndState();
+
+ // Return success
+ return true;
+}
+
+
// ------------------------------------------------------------------------------
// OOO U U TTTTT EEEEE RRRR
// O O U U T E R R
@@ -66,111 +135,22 @@
// 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
+// CCCC OOO N N TTTTT RRRR OOO L
+// C O O NN N T R R O O L
+// C O O N N N T RRRR O O L
+// C O O N NN T R R O O L
+// CCCC OOO N N T R R OOO LLLLL
+//
+// L OOO OOO PPPP
+// L O O O O P P
+// L O O O O PPPP
+// L O O O O P
+// LLLLL OOO OOO P
// ----------------------------------------------------------------------------------
-// This function is the callback that is linked to the "DefaultController" service that
-// is advertised in the main function. This must have arguments that match the
-// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
-// folder)
-//
-// The arument "request" is a structure provided to this service with the following two
-// properties:
-// request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is defined in the
-// file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
-// float64 x The x position of the Crazyflie [metres]
-// float64 y The y position of the Crazyflie [metres]
-// float64 z The z position of the Crazyflie [metres]
-// float64 roll The roll component of the intrinsic Euler angles [radians]
-// float64 pitch The pitch component of the intrinsic Euler angles [radians]
-// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
-// The values in these properties are directly the measurement taken by the Vicon
-// motion capture system of the Crazyflie that is to be controlled by this service
-//
-// request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access to the
-// Vicon measurements of other Crazyflies.
-//
-// The argument "response" is a structure that is expected to be filled in by this
-// service by this function, it has only the following property
-// response.ControlCommand
-// This property is iteself a structure of type "ControlCommand", which is defined in
-// the file "ControlCommand.msg", and has the following properties:
-// float32 roll The command sent to the Crazyflie for the body frame x-axis
-// float32 pitch The command sent to the Crazyflie for the body frame y-axis
-// float32 yaw The command sent to the Crazyflie for the body frame z-axis
-// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
-// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
-//
-// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines"
-// section in the header file, they are:
-// - CF_COMMAND_TYPE_MOTORS
-// - CF_COMMAND_TYPE_RATE
-// - CF_COMMAND_TYPE_ANGLE
-//
-// > For completeing the class exercises it is only required to use
-// the CF_COMMAND_TYPE_RATE option.
-//
-// > For the CF_COMMAND_TYPE_RATE optoin:
-// 1) the ".roll", ".ptich", and ".yaw" properties of
-// "response.ControlCommand" specify the angular rate in
-// [radians/second] that will be requested from the PID controllers
-// running in the Crazyflie 2.0 firmware.
-// 2) the ".motorCmd1" to ".motorCmd4" properties of
-// "response.ControlCommand" are the baseline motor commands
-// requested from the Crazyflie, with the adjustment for body rates
-// being added on top of this in the firmware (i.e., as per the
-// code of the "distribute_power" found in the firmware).
-// 3) the axis convention for the roll, pitch, and yaw body rates
-// returned in "response.ControlCommand" should use right-hand
-// coordinate axes with x-forward and z-upwards (i.e., the positive
-// z-axis is aligned with the direction of positive thrust). To
-// assist, the following is an ASCII art of this convention.
-//
-// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
-//
-// > This is a top view,
-// > M1 to M4 stand for Motor 1 to Motor 4,
-// > "CW" indicates that the motor rotates Clockwise,
-// > "CCW" indicates that the motor rotates Counter-Clockwise,
-// > By right-hand axis convention, the positive z-direction points our of the screen,
-// > This being a "top view" means tha the direction of positive thrust produced
-// by the propellers is also out of the screen.
-//
-// ____ ____
-// / \ / \
-// (CW) | M4 | x | M1 | (CCW)
-// \____/\ ^ /\____/
-// \ \ | / /
-// \ \ | / /
-// \ \______ | ______/ /
-// \ | /
-// | | |
-// y <-------------o |
-// | |
-// / _______________ \
-// / / \ \
-// / / \ \
-// ____/ / \ \____
-// / \/ \/ \
-// (CCW) | M3 | | M2 | (CW)
-// \____/ \____/
-//
-//
-//
-// This function WILL NEED TO BE edited for successful completion of the classroom exercise
+
+
+// THE MAIN CONTROL FUNCTION CALLED FROM THE FLYING AGENT CLIENT
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
@@ -178,6 +158,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// computation here, or you may find it convienient to create functions
// to keep you code cleaner
+ // Increment time
+ m_time_in_seconds += m_control_deltaT;
+
// Define a local array to fill in with the state error
float stateErrorInertial[9];
@@ -228,86 +211,104 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
-
- // YAW CONTROLLER
-
- // Perform the "-Kx" LQR computation for the yaw rate
- // to respond with
- float yawRate_forResponse = 0;
- for(int i = 0; i < 9; ++i)
+ if (m_current_state == DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS)
{
- yawRate_forResponse -= m_gainMatrixYawRate[i] * stateErrorBody[i];
+ // Compute the "spinning" thrust
+ float thrust_for_spinning = 1000.0 + min(0.4,m_time_in_seconds) * 10000.0;
+
+ response.controlOutput.roll = 0.0;
+ response.controlOutput.pitch = 0.0;
+ response.controlOutput.yaw = 0.0;
+ response.controlOutput.motorCmd1 = thrust_for_spinning;
+ response.controlOutput.motorCmd2 = thrust_for_spinning;
+ response.controlOutput.motorCmd3 = thrust_for_spinning;
+ response.controlOutput.motorCmd4 = thrust_for_spinning;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
}
- // Put the computed yaw rate into the "response" variable
- response.controlOutput.yaw = yawRate_forResponse;
+ else
+ {
+
+ // YAW CONTROLLER
+ // Perform the "-Kx" LQR computation for the yaw rate
+ // to respond with
+ float yawRate_forResponse = 0;
+ for(int i = 0; i < 9; ++i)
+ {
+ yawRate_forResponse -= m_gainMatrixYawRate[i] * stateErrorBody[i];
+ }
+ // Put the computed yaw rate into the "response" variable
+ response.controlOutput.yaw = yawRate_forResponse;
- // ALITUDE CONTROLLER (i.e., z-controller)
-
- // Perform the "-Kx" LQR computation for the thrust adjustment
- // to use for computing the response with
- float thrustAdjustment = 0;
- for(int i = 0; i < 9; ++i)
- {
- thrustAdjustment -= m_gainMatrixThrust[i] * stateErrorBody[i];
- }
- // Add the feed-forward thrust before putting in the response
- float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
- float thrust_per_motor = thrustAdjustment + 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_per_motor);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrust_per_motor);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_per_motor);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_per_motor);
-
- // BODY FRAME X CONTROLLER
+ // ALITUDE CONTROLLER (i.e., z-controller)
+
+ // Perform the "-Kx" LQR computation for the thrust adjustment
+ // to use for computing the response with
+ float thrustAdjustment = 0;
+ for(int i = 0; i < 9; ++i)
+ {
+ thrustAdjustment -= m_gainMatrixThrust[i] * stateErrorBody[i];
+ }
- // Perform the "-Kx" LQR computation for the pitch rate
- // to respoond with
- float pitchRate_forResponse = 0;
- for(int i = 0; i < 9; ++i)
- {
- pitchRate_forResponse -= m_gainMatrixPitchRate[i] * stateErrorBody[i];
- }
- // Put the computed pitch rate into the "response" variable
- response.controlOutput.pitch = pitchRate_forResponse;
+ // Add the feed-forward thrust before putting in the response
+ float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
+ float thrust_per_motor = thrustAdjustment + 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_per_motor);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrust_per_motor);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_per_motor);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_per_motor);
+
+
+ // BODY FRAME X CONTROLLER
+
+ // Perform the "-Kx" LQR computation for the pitch rate
+ // to respoond with
+ float pitchRate_forResponse = 0;
+ for(int i = 0; i < 9; ++i)
+ {
+ pitchRate_forResponse -= m_gainMatrixPitchRate[i] * stateErrorBody[i];
+ }
+ // Put the computed pitch rate into the "response" variable
+ response.controlOutput.pitch = pitchRate_forResponse;
- // BODY FRAME Y CONTROLLER
+ // BODY FRAME Y CONTROLLER
- // Instantiate the local variable for the roll rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
-
+ // Instantiate the local variable for the roll rate that will be requested
+ // from the Crazyflie's on-baord "inner-loop" controller
+
- // Perform the "-Kx" LQR computation for the roll rate
- // to respoond with
- float rollRate_forResponse = 0;
- for(int i = 0; i < 9; ++i)
- {
- rollRate_forResponse -= m_gainMatrixRollRate[i] * stateErrorBody[i];
+ // Perform the "-Kx" LQR computation for the roll rate
+ // to respoond with
+ float rollRate_forResponse = 0;
+ for(int i = 0; i < 9; ++i)
+ {
+ rollRate_forResponse -= m_gainMatrixRollRate[i] * stateErrorBody[i];
+ }
+ // Put the computed roll rate into the "response" variable
+ response.controlOutput.roll = rollRate_forResponse;
+
+
+
+ // PREPARE AND RETURN THE VARIABLE "response"
+
+ /*choosing the Crazyflie onBoard controller type.
+ it can 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;
}
- // Put the computed roll rate into the "response" variable
- response.controlOutput.roll = rollRate_forResponse;
-
-
-
- // PREPARE AND RETURN THE VARIABLE "response"
-
- /*choosing the Crazyflie onBoard controller type.
- it can either be Motor, Rate or Angle based */
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
-
@@ -401,30 +402,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// BBBB OOO DDDD Y F R R A A M M EEEEE
// ----------------------------------------------------------------------------------
-// The arguments for this function are as follows:
-// stateInertial
-// This is an array of length 9 with the estimates the error of of the following values
-// relative to the sepcifed setpoint:
-// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
-// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
-// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
-// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
-// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
-// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
-// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
-// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
-// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
-//
-// stateBody
-// This is an empty array of length 9, this function should fill in all elements of this
-// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
-// position and (x,y) velocities are rotated into the body frame.
-//
-// yaw_measured
-// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
-// the Vicon motion capture system
-//
-// This function WILL NEED TO BE edited for successful completion of the classroom exercise
+// ROTATES THE (x,y) COMPONENTS BY THE PROVIDED "yaw" ANGLE
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
{
@@ -465,11 +443,10 @@ void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float y
// CCCC OOO N N V EEEEE R R SSSS III OOO N N
// ----------------------------------------------------------------------------------
-// This function DOES NEED TO BE edited for successful completion of
-// the exercise
+// CONVERTS A THURST IN NEWTONS TO A 16-BIT NUMBER
float computeMotorPolyBackward(float thrust)
{
- // Compute the 16-but command that would produce the requested
+ // Compute the 16-bit command that would produce the requested
// "thrust" based on the quadratic mapping that is described
// by the coefficients in the "yaml_motorPoly" variable.
float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
@@ -574,11 +551,30 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
response.setpointWithHeader.y = m_setpoint[1];
response.setpointWithHeader.z = m_setpoint[2];
response.setpointWithHeader.yaw = m_setpoint[3];
+ // Put the current state into the "buttonID" field
+ response.buttonID = m_current_state;
// Return
return true;
}
+// PUBLISH THE CURRENT SETPOINT SO THAT THE NETWORK IS UPDATED
+void publishCurrentSetpointAndState()
+{
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.x = m_setpoint[0];
+ msg.y = m_setpoint[1];
+ msg.z = m_setpoint[2];
+ msg.yaw = m_setpoint[3];
+ // Put the current state into the "buttonID" field
+ response.buttonID = m_current_state;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
+
+
@@ -656,6 +652,8 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
+
+
// ----------------------------------------------------------------------------------
// L OOO A DDDD
// L O O A A D D
@@ -765,6 +763,9 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// gravity (i.e., mg) in units of [Newtons]
m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
+ // > Conver the control frequency to a delta T
+ m_control_deltaT = 1.0 / yaml_control_frequency;
+
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[DEFAULT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
}
@@ -955,7 +956,7 @@ int main(int argc, char* argv[])
// 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);
+ ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);
@@ -967,7 +968,7 @@ int main(int argc, char* argv[])
// 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("DefaultController", calculateControlOutput);
+ ros::ServiceServer controllerService = nodeHandle.advertiseService("DefaultController", calculateControlOutput);
// Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
// type variable that subscribes to the "GUIButton" topic and calls the class
@@ -977,6 +978,28 @@ int main(int argc, char* argv[])
+ // Instantiate the local variable "service" to be a "ros::ServiceServer"
+ // type variable that advertises the service called:
+ // >> "RequestManoeuvre"
+ // This service has the input-output behaviour defined in the
+ // "IntIntService.srv" file (located in the "srv" folder).
+ // This service, when called, is provided with what manoeuvre
+ // is requested and responds with the duration that menoeuvre
+ // will take to perform (in milliseconds)
+ ros::ServiceServer requestManoeuvreService = nodeHandle.advertiseService("RequestManoeuvre", requestManoeuvreCallback);
+
+
+
+ // Instantiate the class variable "m_stateChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "SetpointChanged" and is a message with 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 for this controller.
+ m_stateChangedPublisher = nodeHandle.advertise<IntWithHeader>("StateChanged", 1);
+
+
+
// Print out some information to the user.
ROS_INFO("[DEFAULT CONTROLLER] Service ready :-)");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 00a01201415175945323f0988e5e2fc5abfec9d3..e48d3d11dd53c6ce5935c840fbff1f40759c88a2 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -59,178 +59,128 @@
+// ----------------------------------------------------------------------------------
+// M M OOO TTTTT III OOO N N
+// MM MM O O T I O O NN N
+// M M M O O T I O O N N N
+// M M O O T I O O N NN
+// M M OOO T III OOO N N
+//
+// CCCC A PPPP TTTTT U U RRRR EEEEE
+// C A A P P T U U R R E
+// C A A PPPP T U U RRRR EEE
+// C AAAAA P T U U R R E
+// CCCC A A P T UUU R R EEEEE
+// ----------------------------------------------------------------------------------
-
-
-
-
-
-
-
-
-// void takeOffCF(CrazyflieData& current_local_coordinates) //local because the setpoint is in local coordinates
-// {
-// // set the setpoint and call safe controller
-// Setpoint setpoint_msg;
-// setpoint_msg.x = current_local_coordinates.x; // previous one
-// setpoint_msg.y = current_local_coordinates.y; //previous one
-// setpoint_msg.z = current_local_coordinates.z + yaml_take_off_distance; //previous one plus some offset
-// // setpoint_msg.yaw = current_local_coordinates.yaw; //previous one
-// setpoint_msg.yaw = 0.0;
-// safeControllerServiceSetpointPublisher.publish(setpoint_msg);
-// ROS_INFO("[FLYING AGENT CLIENT] Take OFF:");
-// ROS_INFO("[FLYING AGENT CLIENT] ------Current coordinates:");
-// ROS_INFO("[FLYING AGENT CLIENT] X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
-// ROS_INFO("[FLYING AGENT CLIENT] ------New coordinates:");
-// ROS_INFO("[FLYING AGENT CLIENT] X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
-
-// // now, use safe controller to go to that setpoint
-// loadSafeController();
-// setInstantController(SAFE_CONTROLLER);
-// // when do we finish? after some time with delay?
-
-// // update variable that keeps track of current setpoint
-// setCurrentSafeSetpoint(setpoint_msg);
-// }
-
-// void landCF(CrazyflieData& current_local_coordinates)
-// {
-// // set the setpoint and call safe controller
-// Setpoint setpoint_msg;
-// setpoint_msg.x = current_local_coordinates.x; // previous one
-// setpoint_msg.y = current_local_coordinates.y; //previous one
-// setpoint_msg.z = yaml_landing_distance; //previous one plus some offset
-// setpoint_msg.yaw = current_local_coordinates.yaw; //previous one
-// safeControllerServiceSetpointPublisher.publish(setpoint_msg);
-
-// // now, use safe controller to go to that setpoint
-// loadSafeController();
-// setInstantController(SAFE_CONTROLLER);
-// setCurrentSafeSetpoint(setpoint_msg);
-// }
-
-
-
-
-// void goToControllerSetpoint()
-// {
-// safeControllerServiceSetpointPublisher.publish(controller_setpoint);
-// setCurrentSafeSetpoint(controller_setpoint);
-// }
-
-
-//is called when new data from Vicon arrives
+// CALLBACK RUN EVERY TIME NEW MOTION CAPTURE DATA IS RECEIVED
void viconCallback(const ViconData& viconData)
{
- // NOTE: THIS FUNCTION IS CALL AT THE FREQUENCY OF THE MOTION
- // CAPTURE SYSTEM. HENCE ANY OPTERATIONS PERFORMED IN
- // THIS FUNCTION MUST BE NON-BLOCKING.
+ // NOTE: THIS FUNCTION IS CALL AT THE FREQUENCY OF THE MOTION
+ // CAPTURE SYSTEM. HENCE ANY OPTERATIONS PERFORMED IN
+ // THIS FUNCTION MUST BE NON-BLOCKING.
- // Initialise a counter of consecutive frames of motion
- // capture data where the agent is occuled
- static int number_of_consecutive_occulsions = 0;
+ // Initialise a counter of consecutive frames of motion
+ // capture data where the agent is occuled
+ static int number_of_consecutive_occulsions = 0;
- // Initilise a variable for the pose data of this agent
- CrazyflieData poseDataForThisAgent;
+ // Initilise a variable for the pose data of this agent
+ CrazyflieData poseDataForThisAgent;
- // Extract the pose data from the full motion capture array
- // NOTE: that if the return index is a negative then this
- // indicates that the pose data was not found.
- m_poseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, m_context.crazyflieName , m_poseDataIndex , poseDataForThisAgent );
+ // Extract the pose data from the full motion capture array
+ // NOTE: that if the return index is a negative then this
+ // indicates that the pose data was not found.
+ m_poseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, m_context.crazyflieName , m_poseDataIndex , poseDataForThisAgent );
- // Detecting time-out of the motion capture data
- // > Update the flag
- m_isAvailable_mocap_data = true;
- // > Stop any previous instance that might still be running
- m_timer_mocap_timeout_check.stop();
- // > Set the period again (second argument is reset)
- m_timer_mocap_timeout_check.setPeriod( ros::Duration(yaml_mocap_timeout_duration), true);
- // > Start the timer again
- m_timer_mocap_timeout_check.start();
-
-
- // Only continue if:
- // (1) the pose for this agent was found, and
- // (2) the flying state is something other than MOTORS-OFF
- if (
- (m_poseDataIndex >= 0)
- and
- (m_flying_state != STATE_MOTORS_OFF)
- and
- (m_controllers_avialble)
- )
- {
- // Initliase the "Contrller" service call variable
- Controller controllerCall;
-
- // Fill in the pose data for this agent
- controllerCall.request.ownCrazyflie = poseDataForThisAgent;
-
- // Initialise a node handle, needed for starting timers
- ros::NodeHandle nodeHandle("~");
-
- // If the object is NOT occluded,
- // then proceed to make the "Controller Service Call" that
- // compute the controller output.
- if(!poseDataForThisAgent.occluded)
- {
- // Reset the "consecutive occulsions" flag
- number_of_consecutive_occulsions = 0;
+ // Detecting time-out of the motion capture data
+ // > Update the flag
+ m_isAvailable_mocap_data = true;
+ // > Stop any previous instance that might still be running
+ m_timer_mocap_timeout_check.stop();
+ // > Set the period again (second argument is reset)
+ m_timer_mocap_timeout_check.setPeriod( ros::Duration(yaml_mocap_timeout_duration), true);
+ // > Start the timer again
+ m_timer_mocap_timeout_check.start();
+
+
+ // Only continue if:
+ // (1) the pose for this agent was found, and
+ // (2) the flying state is something other than MOTORS-OFF
+ if (
+ (m_poseDataIndex >= 0)
+ and
+ (m_flying_state != STATE_MOTORS_OFF)
+ and
+ (m_controllers_avialble)
+ )
+ {
+ // Initliase the "Contrller" service call variable
+ Controller controllerCall;
- // PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
- if ( getInstantController() != DEFAULT_CONTROLLER )
- {
- if ( !safetyCheck(poseDataForThisAgent) )
- {
- setInstantController(DEFAULT_CONTROLLER);
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] safety check failed, switching to DEFAULT CONTROLLER");
- }
- }
+ // Fill in the pose data for this agent
+ controllerCall.request.ownCrazyflie = poseDataForThisAgent;
- // Initialise a local boolean success variable
- bool isSuccessful_controllerCall = false;
+ // If the object is NOT occluded,
+ // then proceed to make the "Controller Service Call" that
+ // compute the controller output.
+ if(!poseDataForThisAgent.occluded)
+ {
+ // Reset the "consecutive occulsions" flag
+ number_of_consecutive_occulsions = 0;
+ // PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
+ if ( getInstantController() != DEFAULT_CONTROLLER )
+ {
+ if ( !safetyCheck_on_positionAndTilt(poseDataForThisAgent) )
+ {
+ setInstantController(DEFAULT_CONTROLLER);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] safety check failed, switching to DEFAULT CONTROLLER");
+ }
+ }
+ // Initialise a local boolean success variable
+ bool isSuccessful_controllerCall = false;
- isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
+ // Call the controller service client
+ isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
- // Ensure success and enforce safety
- if(!isSuccessful_controllerCall)
- {
- // Let the user know that the controller call failed
- ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
-
- // Switch to the default controller,
- // if it was not already the active controller
- if ( getInstantController() != DEFAULT_CONTROLLER )
- {
- // Set the DEFAULT controller to be active
- setInstantController(DEFAULT_CONTROLLER);
- // Try the controller call
- isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
- // Inform the user is not successful
- if ( !isSuccessful_controllerCall )
- {
- ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Also failed to call the DEFAULT controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
- }
- }
- }
+ // Ensure success and enforce safety
+ if(!isSuccessful_controllerCall)
+ {
+ // Let the user know that the controller call failed
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
+
+ // Switch to the default controller,
+ // if it was not already the active controller
+ if ( getInstantController() != DEFAULT_CONTROLLER )
+ {
+ // Set the DEFAULT controller to be active
+ setInstantController(DEFAULT_CONTROLLER);
+ // Try the controller call
+ isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
+ // Inform the user is not successful
+ if ( !isSuccessful_controllerCall )
+ {
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Also failed to call the DEFAULT controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
+ }
+ }
+ }
- // Send the command to the CrazyRadio
- // > IF SUCCESSFUL
- if (isSuccessful_controllerCall)
- {
- m_commaNdfOrsendiNgTocrazyflIepublisher.publish(controllerCall.response.controlOutput);
- }
- // > ELSE SEND ZERO OUTPUT COMMAND
- else
- {
- // Send the command to turn the motors off
- sendZeroOutputCommandForMotors();
- // And change the state to motor-off
- changeFlyingStateTo(STATE_MOTORS_OFF);
- }
+ // Send the command to the CrazyRadio
+ // > IF SUCCESSFUL
+ if (isSuccessful_controllerCall)
+ {
+ m_commaNdfOrsendiNgTocrazyflIepublisher.publish(controllerCall.response.controlOutput);
+ }
+ // > ELSE SEND ZERO OUTPUT COMMAND
+ else
+ {
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+ // And change the state to motor-off
+ requestChangeFlyingStateTo(STATE_MOTORS_OFF);
+ }
}
else
{
@@ -246,17 +196,17 @@ void viconCallback(const ViconData& viconData)
// Update the flag
m_isOcculded_mocap_data = true;
// Send the command to turn the motors off
- sendZeroOutputCommandForMotors();
- // Update the flying state
- changeFlyingStateTo( STATE_MOTORS_OFF );
+ sendZeroOutputCommandForMotors();
+ // Update the flying state
+ requestChangeFlyingStateTo( STATE_MOTORS_OFF );
}
- } // END OF: "if(!global.occluded)"
+ } // END OF: "if(!poseDataForThisAgent.occluded)"
- }
- else
- {
- // Send the command to turn the motors off
- sendZeroOutputCommandForMotors();
+ }
+ else
+ {
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
} // END OF: "if ( (m_poseDataIndex >= 0) and (m_flying_state != STATE_MOTORS_OFF) )"
@@ -338,7 +288,7 @@ void coordinatesToLocal(CrazyflieData& cf)
void timerCallback_mocap_timeout_check(const ros::TimerEvent&)
{
// Update the flag
- m_isAvailable_mocap_data = true;
+ m_isAvailable_mocap_data = false;
// Inform the user
ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Motion Capture Data has been unavailable for " << yaml_mocap_timeout_duration << " seconds." );
// Ensure that the motors are turned off
@@ -347,7 +297,7 @@ void timerCallback_mocap_timeout_check(const ros::TimerEvent&)
// Send the command to turn the motors off
sendZeroOutputCommandForMotors();
// Update the flying state
- changeFlyingStateTo( STATE_MOTORS_OFF );
+ requestChangeFlyingStateTo( STATE_MOTORS_OFF );
}
}
@@ -363,42 +313,75 @@ void sendZeroOutputCommandForMotors()
+// ----------------------------------------------------------------------------------
+// SSSS A FFFFF EEEEE TTTTT Y Y
+// S A A F E T Y Y
+// SSS A A FFF EEE T Y
+// S AAAAA F E T Y
+// SSSS A A F EEEEE T Y
+//
+// CCCC H H EEEEE CCCC K K SSSS
+// C H H E C K K S
+// C HHHHH EEE C KKK SSS
+// C H H E C K K S
+// CCCC H H EEEEE CCCC K K SSSS
+// ----------------------------------------------------------------------------------
-
-
-
-void changeFlyingStateTo(int new_state)
+// Checks if crazyflie is within allowed area
+bool safetyCheck_on_positionAndTilt(CrazyflieData data)
{
- if(crazyradio_status == CRAZY_RADIO_STATE_CONNECTED)
+ // Check on the X position
+ if((data.x < m_context.localArea.xmin) or (data.x > m_context.localArea.xmax))
{
- ROS_INFO("[FLYING AGENT CLIENT] Change state to: %d", new_state);
- m_flying_state== = new_state;
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] x safety failed");
+ return false;
}
- else
+ // Check on the Y position
+ if((data.y < m_context.localArea.ymin) or (data.y > m_context.localArea.ymax))
{
- ROS_INFO("[FLYING AGENT CLIENT] Disconnected and trying to change state. State goes to MOTORS OFF");
- m_flying_state = STATE_MOTORS_OFF;
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] y safety failed");
+ return false;
+ }
+ // Check on the Z position
+ if((data.z < m_context.localArea.zmin) or (data.z > m_context.localArea.zmax))
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] z safety failed");
+ return false;
}
- // Set the class variable flag that the
- // flying state was changed
- //m_changed_flying_state_flag = true;
-
- // Publish a message with the new flying state
- std_msgs::Int32 flying_state_msg;
- flying_state_msg.data = m_flying_state;
- flyingStatePublisher.publish(flying_state_msg);
-}
-
-
-void takeOffTimerCallback(const ros::TimerEvent&)
-{
- //finished_take_off = true;
-}
+ // Check the title angle (if required)
+ // > The tilt anlge between the body frame and inertial frame z-axis is
+ // give by:
+ // tilt angle = 1 / ( cos(roll)*cos(pitch) )
+ // > But this would be too sensitve to a divide by zero error, so instead
+ // we just check if each angle separately exceeds the limit
+ if(yaml_isEnabled_strictSafety)
+ {
+ // Check on the ROLL angle
+ if(
+ (data.roll > m_maxTiltAngle_for_strictSafety_redians)
+ or
+ (data.roll < -m_maxTiltAngle_for_strictSafety_redians)
+ )
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] roll too big.");
+ return false;
+ }
+ // Check on the PITCH angle
+ if(
+ (data.pitch > m_maxTiltAngle_for_strictSafety_redians)
+ or
+ (data.pitch < -m_maxTiltAngle_for_strictSafety_redians)
+ )
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] pitch too big.");
+ return false;
+ }
+ }
-void landTimerCallback(const ros::TimerEvent&)
-{
- //finished_land = true;
+ // If the code makes it to here then all the safety checks passed,
+ // Hence return "true"
+ return true;
}
@@ -408,89 +391,252 @@ void landTimerCallback(const ros::TimerEvent&)
+// ----------------------------------------------------------------------------------
+// FFFFF L Y Y III N N GGGG
+// F L Y Y I NN N G
+// FFF L Y I N N N G
+// F L Y I N NN G G
+// F LLLLL Y III N N GGGG
+//
+// SSSS TTTTT A TTTTT EEEEE
+// S T A A T E
+// SSS T A A T EEE
+// S T AAAAA T E
+// SSSS T A A T EEEEE
+// ----------------------------------------------------------------------------------
+void requestChangeFlyingStateTo(int new_state)
+{
+ if(m_crazyradio_status != CRAZY_RADIO_STATE_CONNECTED)
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] Disconnected and trying to change state. State goes to MOTORS OFF");
+ m_flying_state = STATE_MOTORS_OFF;
+ }
+ else
+ {
+ // Switch between the possible new states
+ switch (new_state)
+ {
+ case STATE_TAKE_OFF:
+ {
+ requestChangeFlyingStateToTakeOff();
+ break;
+ }
+
+ case STATE_FLYING:
+ {
+ // This should never be called
+ break;
+ }
+
+ case STATE_LAND:
+ {
+ requestChangeFlyingStateToLand();
+ break;
+ }
+
+ case STATE_MOTORS_OFF:
+ {
+ ROS_INFO("[FLYING AGENT CLIENT] Change state to MOTORS OFF");
+ m_flying_state = new_state;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Request state of " << new_state << " not recognised. Hence changing to MOTORS OFF.");
+ m_flying_state = new_state;
+ break;
+ }
+ }
+ }
+ // Publish a message with the new flying state
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ flyingStatePublisher.publish(flying_state_msg);
+}
+void requestChangeFlyingStateToTakeOff()
+{
+ // Only allow taking off from the MOTORS OFF state
+ if (m_flying_state != STATE_MOTORS_OFF)
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Request to TAKE OFF was not implemented because the current state is NOT the MOTORS OFF state.");
+ }
+ else
+ {
+ // Check that the Motion Capture data is available
+ if ( m_isAvailable_mocap_data and !m_isOcculded_mocap_data )
+ {
+ // Check whether a "controller" take-off should
+ // be performed, and that not already in the
+ // "take-off" state
+ if (
+ (yaml_shouldPerfom_takeOff_with_defaultController)
+ and
+ (m_flying_state != STATE_TAKE_OFF)
+ )
+ {
+ // Call the service offered by the default
+ // controller for how long a take-off will take
+ dfall_pkg::IntIntService requestManoeurveCall;
+ requestManoeurveCall.request.data = DEFAULT_CONTROLLER_REQUEST_TAKE_OFF;
+ if(m_defaultController_requestManoeuvre.call(requestManoeurveCall))
+ {
+ // Extract the duration
+ float take_off_duration = float(requestManoeurveCall.response.data) / 1000.0;
+ // Start the timer
+ // > Stop any previous instance
+ m_timer_takeoff_complete.stop();
+ // > Set the period again (second argument is reset)
+ m_timer_takeoff_complete.setPeriod( ros::Duration(take_off_duration), true);
+ // > Start the timer
+ m_timer_takeoff_complete.start();
+ // Inform the user
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] Changed state to STATE_TAKE_OFF for a duration of " << take_off_duration << " seconds.");
+ // Update the class variable
+ m_flying_state = STATE_TAKE_OFF;
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Failed to get take-off duration from Default controller. Switching to MOTORS-OFF.");
+ // Update the class variable
+ m_flying_state = STATE_MOTORS_OFF;
+ }
+ }
+ // Otherwise, just switch straight to the
+ // "flying" state
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Changed state directly to STATE_FLYING");
+ // Update the class variable
+ m_flying_state = STATE_FLYING;
+ }
+ }
+ else
+ {
+ // Inform the user of the problem
+ if (!m_isAvailable_mocap_data)
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the motion capture data is unavailable.");
+ }
+ if (m_isOcculded_mocap_data)
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the object is \"long-term\" occuled.");
+ }
+ }
+ }
+}
+void requestChangeFlyingStateToLand()
+{
+ // Only allow landing from the TAKE-OFF and FLYING state
+ if (
+ (m_flying_state != STATE_TAKE_OFF)
+ and
+ (m_flying_state != STATE_FLYING)
+ )
+ {
+ ROS_ERROR("[FLYING AGENT CLIENT] Request to LAND was not implemented because the current state is NOT the TAKE-OFF or FLYING state.");
+ }
+ else
+ {
+ // Check whether a "controller" take-off should
+ // be performed, and that not already in the
+ // "land" state
+ if (
+ (yaml_shouldPerfom_landing_with_defaultController)
+ and
+ (m_flying_state != STATE_LAND)
+ )
+ {
+ // Call the service offered by the default
+ // controller for how long a landing will take
+ dfall_pkg::IntIntService requestManoeurveCall;
+ requestManoeurveCall.request.data = DEFAULT_CONTROLLER_REQUEST_LANDING;
+ if(m_defaultController_requestManoeuvre.call(requestManoeurveCall))
+ {
+ // Extract the duration
+ float land_duration = float(requestManoeurveCall.response.data) / 1000.0;
+ // Start the timer
+ // > Stop any previous instance
+ m_timer_land_complete.stop();
+ // > Set the period again (second argument is reset)
+ m_timer_land_complete.setPeriod( ros::Duration(take_off_duration), true);
+ // > Start the timer
+ m_timer_land_complete.start();
+ // Inform the user
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] Changed state to STATE_TAKE_OFF for a duration of " << land_duration << " seconds.");
+ // Update the class variable
+ m_flying_state = STATE_LAND;
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Failed to get take-off duration from Default controller. Switching to MOTORS-OFF.");
+ // Update the class variable
+ m_flying_state = STATE_MOTORS_OFF;
+ }
+ }
+ // Otherwise, just switch straight to the
+ // "motors off" state
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Changed state directly to STATE_MOTORS_OFF");
+ // Update the class variable
+ m_flying_state = STATE_MOTORS_OFF;
+ }
+ }
+}
+void timerCallback_takeoff_complete(const ros::TimerEvent&)
+{
+ // Only change to flying if still in the take-off state
+ if (m_flying_state == STATE_TAKE_OFF)
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Take-off complete, changed state to STATE_FLYING");
+ // Update the class variable
+ m_flying_state = STATE_FLYING;
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Take-off duration elapsed but the agent is no longer in STATE_TAKE_OFF.");
+ }
+}
-
-
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// L OOO A DDDD
-// L O O A A D D
-// L O O A A D D
-// L O O AAAAA D D
-// LLLLL OOO A A DDDD
-//
-//
-// CCCC OOO N N TTTTT RRRR OOO L L EEEEE RRRR
-// C O O NN N T R R O O L L E R R
-// C O O N N N T RRRR O O L L EEE RRRR
-// C O O N NN T R R O O L L E R R
-// CCCC OOO N N T R R OOO LLLLL LLLLL EEEEE R R
-// ----------------------------------------------------------------------------------
-
-
-// CREATE A "CONTROLLER" TYPE SERVICE CLIENT
-// NOTE: that in the "ros::service::createClient" function the
-// second argument is a boolean that specifies whether the
-// service is persistent or not. In the ROS documentation a
-// persistent connection is described as:
-// "Persistent connections should be used carefully. They greatly
-// improve performance for repeated requests, but they also make
-// your client more fragile to service failures. Clients using
-// persistent connections should implement their own reconnection
-// logic in the event that the persistent connection fails."
-void loadController( std::string paramter_name , ros::ServiceClient& serviceClient )
+void timerCallback_land_complete(const ros::TimerEvent&)
{
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
- ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
+ // Only change to flying if still in the take-off state
+ if (m_flying_state == STATE_LAND)
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Land complete, changed state to STATE_MOTORS_OFF");
+ // Update the class variable
+ m_flying_state = STATE_MOTORS_OFF;
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[FLYING AGENT CLIENT] Land duration elapsed but the agent is no longer in STATE_LAND.");
+ }
+}
+
- std::string controllerName;
- if(!nodeHandle.getParam(paramter_name, controllerName))
- {
- ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to get \"" << paramter_name << "\" paramter");
- return;
- }
- serviceClient = ros::service::createClient<Controller>(controllerName, true);
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded service: " << serviceClient.getService() << ", valid: " << serviceClient.isValid() << ", exists: " << serviceClient.exists() );
-}
-void timerCallback_for_creaTeaLlcontrollerServiceClients(const ros::TimerEvent&)
-{
- // INITIALISE ALL THE CONTROLLER SERVICE CLIENTS
- loadController( "defaultController" , defaultController );
- loadController( "studentController" , studentController );
- loadController( "tuningController" , tuningController );
- loadController( "pickerController" , pickerController );
- loadController( "templateController" , templateController );
- // Check that at least the default controller is available
- // > Setting the flag accordingly
- if (defaultController)
- {
- m_controllers_avialble = true;
- }
- else
- {
- m_controllers_avialble = false;
- // Inform the user of the problem
- ROS_ERROR("[FLYING AGENT CLIENT] The default controller service client (and pressumably all other controllers) could NOT be created.");
- }
-}
@@ -513,52 +659,60 @@ void timerCallback_for_creaTeaLlcontrollerServiceClients(const ros::TimerEvent&)
// ----------------------------------------------------------------------------------
-
-void sendMessageUsingController(int controller)
-{
- // Send a message on the topic for informing the Flying
- // Agent GUI about this update
- std_msgs::Int32 controller_used_msg;
- controller_used_msg.data = controller;
- controllerUsedPublisher.publish(controller_used_msg);
-}
-
-void setInstantController(int controller) //for right now, temporal use
+// THIS SETS THE CONTROLLER THAT IT ACTUALLY BEING USED
+// > During take-off and landing this function is
+// called to switch to the "Default" controller
+// > The highlighting in the "Flying Agent GUI" should
+// reflect the instant controller
+void setInstantController(int controller)
{
// Update the class variable
m_instant_controller = controller;
-
-
+ // Point to the correct service client
switch(controller)
{
case DEFAULT_CONTROLLER:
- m_instant_controller_service_client = &defaultController;
+ m_instant_controller_service_client = &m_defaultController;
break;
case STUDENT_CONTROLLER:
- m_instant_controller_service_client = &studentController;
+ m_instant_controller_service_client = &m_studentController;
break;
case TUNING_CONTROLLER:
- m_instant_controller_service_client = &tuningController;
+ m_instant_controller_service_client = &m_tuningController;
break;
case PICKER_CONTROLLER:
- m_instant_controller_service_client = &pickerController;
+ m_instant_controller_service_client = &m_pickerController;
break;
case TEMPLATE_CONTROLLER:
- m_instant_controller_service_client = &templateController;
+ m_instant_controller_service_client = &m_templateController;
break;
default:
break;
}
- sendMessageUsingController(controller);
+ // Publish a message that informs the "Flying Agent
+ // GUI" about this update to the instant controller
+ std_msgs::Int32 controller_used_msg;
+ controller_used_msg.data = controller;
+ controllerUsedPublisher.publish(controller_used_msg);
}
+
+// THIS SIMPLY RETRIEVES THE CLASS VARIABLE
int getInstantController()
{
return m_instant_controller;
}
+
+// THIS SETS THE CONTROLLER THAT IS NOMINALLY SELECTED
+// > This is the controller that will be use as the
+// "instant controller" when in the "flying" state.
+// > But during take-off and landing, the "Default"
+// controller is used, and this keeps track of which
+// controller to switch to after those phases are
+// complete
void setControllerNominallySelected(int controller)
{
// Update the class variable
@@ -566,13 +720,19 @@ void setControllerNominallySelected(int controller)
// If in state "MOTORS-OFF" or "FLYING",
// then the change is instant.
- if(m_flying_state == STATE_MOTORS_OFF || m_flying_state == STATE_FLYING)
+ if (
+ (m_flying_state == STATE_MOTORS_OFF)
+ or
+ (m_flying_state == STATE_FLYING)
+ )
{
setInstantController(controller);
}
}
+
+// THIS SIMPLY RETRIEVES THE CLASS VARIABLE
int getControllerNominallySelected()
{
return m_controller_nominally_selected;
@@ -587,6 +747,31 @@ int getControllerNominallySelected()
+// ----------------------------------------------------------------------------------
+// SSSS TTTTT A TTTTT EEEEE
+// S T A A T E
+// SSS T A A T EEE
+// S T AAAAA T E
+// SSSS T A A T EEEEE
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+//
+// FFFFF RRRR OOO M M GGGG U U III
+// F R R O O MM MM G U U I
+// FFF RRRR O O M M M G U U I
+// F R R O O M M G G U U I
+// F R R OOO M M GGGG UUU III
+// ----------------------------------------------------------------------------------
+
+
+
+// THE CALLBACK THAT A NEW FLYING STATE WAS REQUESTED
+// > These requests come from the "Flying Agent GUI"
+// > The options are: {take-off,land,motor-off,controller}
void flyingStateRequestCallback(const IntWithHeader & msg) {
// Check whether the message is relevant
@@ -641,22 +826,16 @@ void flyingStateRequestCallback(const IntWithHeader & msg) {
case CMD_CRAZYFLY_TAKE_OFF:
ROS_INFO("[FLYING AGENT CLIENT] TAKE_OFF Command received");
- if(m_flying_state == STATE_MOTORS_OFF)
- {
- changeFlyingStateTo(STATE_TAKE_OFF);
- }
+ requestChangeFlyingStateTo(STATE_TAKE_OFF);
break;
case CMD_CRAZYFLY_LAND:
ROS_INFO("[FLYING AGENT CLIENT] LAND Command received");
- if(m_flying_state != STATE_MOTORS_OFF)
- {
- changeFlyingStateTo(STATE_LAND);
- }
+ requestChangeFlyingStateTo(STATE_LAND);
break;
case CMD_CRAZYFLY_MOTORS_OFF:
ROS_INFO("[FLYING AGENT CLIENT] MOTORS_OFF Command received");
- changeFlyingStateTo(STATE_MOTORS_OFF);
+ requestChangeFlyingStateTo(STATE_MOTORS_OFF);
break;
default:
@@ -676,7 +855,7 @@ void flyingStateRequestCallback(const IntWithHeader & msg) {
void crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
ROS_INFO_STREAM("[FLYING AGENT CLIENT] Received message with Crazy Radio Status = " << msg.data );
- crazyradio_status = msg.data;
+ m_crazyradio_status = msg.data;
}
@@ -721,13 +900,6 @@ bool getCurrentFlyingStateServiceCallback(IntIntService::Request &request, IntIn
-
-
-
-
-
-
-
// ----------------------------------------------------------------------------------
// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
// B B A A T T E R R Y Y
@@ -747,7 +919,7 @@ void batteryMonitorStateChangedCallback(std_msgs::Int32 msg)
if (m_flying_state != STATE_MOTORS_OFF)
{
ROS_INFO("[FLYING AGENT CLIENT] low level battery triggered, now landing.");
- changeFlyingStateTo(STATE_LAND);
+ requestChangeFlyingStateTo(STATE_LAND);
}
else
{
@@ -771,76 +943,7 @@ void batteryMonitorStateChangedCallback(std_msgs::Int32 msg)
-// ----------------------------------------------------------------------------------
-// SSSS A FFFFF EEEEE TTTTT Y Y
-// S A A F E T Y Y
-// SSS A A FFF EEE T Y
-// S AAAAA F E T Y
-// SSSS A A F EEEEE T Y
-//
-// CCCC H H EEEEE CCCC K K SSSS
-// C H H E C K K S
-// C HHHHH EEE C KKK SSS
-// C H H E C K K S
-// CCCC H H EEEEE CCCC K K SSSS
-// ----------------------------------------------------------------------------------
-
-// Checks if crazyflie is within allowed area
-bool safetyCheck(CrazyflieData data)
-{
- // Check on the X position
- if((data.x < m_context.localArea.xmin) or (data.x > m_context.localArea.xmax))
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] x safety failed");
- return false;
- }
- // Check on the Y position
- if((data.y < m_context.localArea.ymin) or (data.y > m_context.localArea.ymax))
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] y safety failed");
- return false;
- }
- // Check on the Z position
- if((data.z < m_context.localArea.zmin) or (data.z > m_context.localArea.zmax))
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] z safety failed");
- return false;
- }
-
- // Check the title angle (if required)
- // > The tilt anlge between the body frame and inertial frame z-axis is
- // give by:
- // tilt angle = 1 / ( cos(roll)*cos(pitch) )
- // > But this would be too sensitve to a divide by zero error, so instead
- // we just check if each angle separately exceeds the limit
- if(yaml_isEnabled_strictSafety)
- {
- // Check on the ROLL angle
- if(
- (data.roll > m_maxTiltAngle_for_strictSafety_redians)
- or
- (data.roll < -m_maxTiltAngle_for_strictSafety_redians)
- )
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] roll too big.");
- return false;
- }
- // Check on the PITCH angle
- if(
- (data.pitch > m_maxTiltAngle_for_strictSafety_redians)
- or
- (data.pitch < -m_maxTiltAngle_for_strictSafety_redians)
- )
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] pitch too big.");
- return false;
- }
- }
- // If the code makes it to here then all the safety checks passed,
- // Hence return "true"
- return true;
-}
@@ -916,6 +1019,84 @@ void loadCrazyflieContext()
+// ----------------------------------------------------------------------------------
+// CCCC RRRR EEEEE A TTTTT EEEEE
+// C R R E A A T E
+// C RRRR EEE A A T EEE
+// C R R E AAAAA T E
+// CCCC R R EEEEE A A T EEEEE
+//
+// SSSS EEEEE RRRR V V III CCCC EEEEE
+// S E R R V V I C E
+// SSS EEE RRRR V V I C EEE
+// S E R R V V I C E
+// SSSS EEEEE R R V III CCCC EEEEE
+//
+// CCCC L III EEEEE N N TTTTT
+// C L I E NN N T
+// C L I EEE N N N T
+// C L I E N NN T
+// CCCC LLLLL III EEEEE N N T
+// ----------------------------------------------------------------------------------
+
+
+// CREATE A "CONTROLLER" TYPE SERVICE CLIENT
+// NOTE: that in the "ros::service::createClient" function the
+// second argument is a boolean that specifies whether the
+// service is persistent or not. In the ROS documentation a
+// persistent connection is described as:
+// "Persistent connections should be used carefully. They greatly
+// improve performance for repeated requests, but they also make
+// your client more fragile to service failures. Clients using
+// persistent connections should implement their own reconnection
+// logic in the event that the persistent connection fails."
+void createControllerServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient )
+{
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
+
+ std::string controllerName;
+ if(!nodeHandle.getParam(paramter_name, controllerName))
+ {
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to get \"" << paramter_name << "\" paramter");
+ return;
+ }
+
+ serviceClient = ros::service::createClient<Controller>(controllerName, true);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded service: " << serviceClient.getService() << ", valid: " << serviceClient.isValid() << ", exists: " << serviceClient.exists() );
+}
+
+
+
+void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
+{
+ // INITIALISE ALL THE CONTROLLER SERVICE CLIENTS
+ createControllerServiceClientFromParameterName( "defaultController" , m_defaultController );
+ createControllerServiceClientFromParameterName( "studentController" , m_studentController );
+ createControllerServiceClientFromParameterName( "tuningController" , m_tuningController );
+ createControllerServiceClientFromParameterName( "pickerController" , m_pickerController );
+ createControllerServiceClientFromParameterName( "templateController" , m_templateController );
+
+ // INITIALISE THE SERVICE FOR REQUESTING THE DEFAULT
+ // CONTROLLER TO PERFORM MANOEUVRES
+ createControllerServiceClientFromParameterName( "defaultController_requestManoeuvre" , m_defaultController_requestManoeuvre );
+
+ // Check that at least the default controller is available
+ // > Setting the flag accordingly
+ if (defaultController)
+ {
+ m_controllers_avialble = true;
+ }
+ else
+ {
+ m_controllers_avialble = false;
+ // Inform the user of the problem
+ ROS_ERROR("[FLYING AGENT CLIENT] The default controller service client (and pressumably all other controllers) could NOT be created.");
+ }
+}
+
+
+
// ----------------------------------------------------------------------------------
@@ -991,6 +1172,18 @@ void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
// Flag for whether to use angle for switching to the Safe Controller
yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
yaml_maxTiltAngle_for_strictSafety_degrees = getParameterFloat(nodeHandle_for_paramaters,"maxTiltAngle_for_strictSafety_degrees");
+
+ // Number of consecutive occulsions that will deem the
+ // object as "long-term" occuled
+ consecutive_occulsions_threshold = getParameterInt(nodeHandle_for_paramaters,"consecutive_occulsions_threshold");
+
+ // Flag for whether the take-off should be performed
+ // with the default controller
+ yaml_shouldPerfom_takeOff_with_defaultController = getParameterBool(nodeHandle_for_paramaters,"shouldPerfom_takeOff_with_defaultController");
+
+ // Flag for whether the landing should be performed
+ // with the default controller
+ yaml_shouldPerfom_landing_with_defaultController = getParameterBool(nodeHandle_for_paramaters,"shouldPerfom_landing_with_defaultController");
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched ClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
@@ -1106,7 +1299,7 @@ int main(int argc, char* argv[])
// seconds and in the call back all the controller service
// clients are created.
m_controllers_avialble = false;
- m_timer_for_createAllControllerServiceClients = nodeHandle.createTimer(ros::Duration(3), timerCallback_for_creaTeaLlcontrollerServiceClients, true);
+ m_timer_for_createAllControllerServiceClients = nodeHandle.createTimer(ros::Duration(3), timerCallback_for_createAllcontrollerServiceClients, true);
@@ -1199,7 +1392,7 @@ int main(int argc, char* argv[])
// crazy radio status
- crazyradio_status = CRAZY_RADIO_STATE_DISCONNECTED;
+ m_crazyradio_status = CRAZY_RADIO_STATE_DISCONNECTED;
// publish first flying state data
std_msgs::Int32 flying_state_msg;
@@ -1249,7 +1442,7 @@ int main(int argc, char* argv[])
// Advertise the service for the current flying state
ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("getCurrentFlyingState", getCurrentFlyingStateServiceCallback);
-
+
// Open a ROS "bag" to store data for post-analysis
// std::string package_path;
@@ -1258,9 +1451,9 @@ int main(int argc, char* argv[])
// std::string record_file = package_path + "LoggingFlyingAgentClient.bag";
// bag.open(record_file, rosbag::bagmode::Write);
- ros::spin();
+ ros::spin();
- // Close the ROS "bag" that was opened to store data for post-analysis
+ // Close the ROS "bag" that was opened to store data for post-analysis
//bag.close();
return 0;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
index e3209a6bb10caf461a9405f389461a37224deb48..a1bbba1aaae78e419b69bc4d2a7bb469f2a262e3 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -505,7 +505,7 @@ void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float y
// the exercise
float computeMotorPolyBackward(float thrust)
{
- // Compute the 16-but command that would produce the requested
+ // Compute the 16-bit command that would produce the requested
// "thrust" based on the quadratic mapping that is described
// by the coefficients in the "yaml_motorPoly" variable.
float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);