diff --git a/dfall_ws/src/dfall_pkg/CMakeLists.txt b/dfall_ws/src/dfall_pkg/CMakeLists.txt
index 3191d29bc6013ade732622540dd090f332b95540..142e18444556da668af4d5f2f577c659373b9bbd 100755
--- a/dfall_ws/src/dfall_pkg/CMakeLists.txt
+++ b/dfall_ws/src/dfall_pkg/CMakeLists.txt
@@ -114,6 +114,7 @@ set(SRC_HDRS_QOBJECT_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_INC}/enablecontrollerloadyamlbar.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/mainwindow.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/pickercontrollertab.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/remotecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/rosNodeThread_for_flyingAgentGUI.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/safecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/studentcontrollertab.h
@@ -131,6 +132,7 @@ qt5_wrap_ui(UIS_HDRS_FLYING_AGENT_GUI
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/enablecontrollerloadyamlbar.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/mainwindow.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/pickercontrollertab.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/remotecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/safecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/studentcontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/templatecontrollertab.ui
@@ -321,7 +323,8 @@ add_executable(DemoControllerService src/nodes/DemoControllerService.cpp
add_executable(StudentControllerService src/nodes/StudentControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
-add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
+add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
add_executable(TuningControllerService src/nodes/TuningControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(PickerControllerService src/nodes/PickerControllerService.cpp
@@ -369,6 +372,7 @@ set(FLYING_AGENT_GUI_CPP_SOURCES # compilation of sources
${FLYING_AGENT_GUI_LIB_PATH_SRC}/defaultcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/enablecontrollerloadyamlbar.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/pickercontrollertab.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/remotecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/safecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/studentcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/templatecontrollertab.cpp
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
index 529942c8ac112c59a1de472de706e7ef463a0986..daf227898c1ca5ea81aa9303b06e6dee3b202c71 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
@@ -30,6 +30,7 @@ SOURCES += src/main.cpp\
src/pickercontrollertab.cpp \
src/templatecontrollertab.cpp \
src/tuningcontrollertab.cpp \
+ src/remotecontrollertab.cpp
HEADERS += include/mainwindow.h \
@@ -46,6 +47,7 @@ HEADERS += include/mainwindow.h \
include/templatecontrollertab.h \
include/tuningcontrollertab.h \
include/Constants_for_Qt_compile.h \
+ include/remotecontrollertab.h
@@ -62,6 +64,7 @@ FORMS += forms/mainwindow.ui \
forms/pickercontrollertab.ui \
forms/templatecontrollertab.ui \
forms/tuningcontrollertab.ui \
+ forms/remotecontrollertab.ui
RESOURCES += \
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
index 471a55f3a30fb3afa2075720f8a66d6dfaf53a66..ceeabd07c08162f564703ebf09e27935f1c85ae4 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
@@ -73,6 +73,16 @@
</item>
</layout>
</widget>
+ <widget class="QWidget" name="remote_tab">
+ <attribute name="title">
+ <string>Remote</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
+ <widget class="RemoteControllerTab" name="remote_controller_tab_widget" native="true"/>
+ </item>
+ </layout>
+ </widget>
<widget class="QWidget" name="template_tab">
<attribute name="title">
<string>Template</string>
@@ -118,6 +128,12 @@
<header>templatecontrollertab.h</header>
<container>1</container>
</customwidget>
+ <customwidget>
+ <class>RemoteControllerTab</class>
+ <extends>QWidget</extends>
+ <header>remotecontrollertab.h</header>
+ <container>1</container>
+ </customwidget>
</customwidgets>
<resources/>
<connections/>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
index c358d8befac4a77bff9b211af6cdb3eb9d2fd32c..7bafd6f49857ce5a3ebe291dbe62805069eaf7f8 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
@@ -30,8 +30,8 @@
<property name="spacing">
<number>12</number>
</property>
- <item row="0" column="2">
- <widget class="QPushButton" name="enable_student_button">
+ <item row="1" column="1">
+ <widget class="QPushButton" name="load_yaml_default_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -50,19 +50,38 @@
<height>50</height>
</size>
</property>
- <property name="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QPushButton" name="load_yaml_picker_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
</property>
<property name="text">
- <string>Student</string>
+ <string>Picker</string>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="load_yaml_label">
+ <item row="0" column="0">
+ <widget class="QLabel" name="enable_controller_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -88,59 +107,74 @@
</font>
</property>
<property name="text">
- <string>Load YAML</string>
+ <string>Enable</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="0" column="6">
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
+ <item row="1" column="4">
+ <widget class="QPushButton" name="load_yaml_tuning_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
- <property name="sizeHint" stdset="0">
+ <property name="minimumSize">
<size>
- <width>40</width>
- <height>20</height>
+ <width>60</width>
+ <height>50</height>
</size>
</property>
- </spacer>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Tuning</string>
+ </property>
+ </widget>
</item>
- <item row="0" column="5">
- <widget class="QPushButton" name="enable_template_button">
+ <item row="1" column="0">
+ <widget class="QLabel" name="load_yaml_label">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>60</width>
+ <width>0</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
- <width>180</width>
+ <width>16777215</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
- <weight>50</weight>
- <bold>false</bold>
+ <weight>75</weight>
+ <bold>true</bold>
</font>
</property>
<property name="text">
- <string>Template</string>
+ <string>Load YAML</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="1" column="3">
- <widget class="QPushButton" name="load_yaml_picker_button">
+ <item row="1" column="2">
+ <widget class="QPushButton" name="load_yaml_student_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -160,12 +194,12 @@
</size>
</property>
<property name="text">
- <string>Picker</string>
+ <string>Student</string>
</property>
</widget>
</item>
- <item row="1" column="2">
- <widget class="QPushButton" name="load_yaml_student_button">
+ <item row="1" column="6">
+ <widget class="QPushButton" name="load_yaml_template_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -185,12 +219,12 @@
</size>
</property>
<property name="text">
- <string>Student</string>
+ <string>Template</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QPushButton" name="enable_default_button">
+ <item row="0" column="2">
+ <widget class="QPushButton" name="enable_student_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -216,46 +250,37 @@
</font>
</property>
<property name="text">
- <string>Default</string>
+ <string>Student</string>
</property>
</widget>
</item>
- <item row="0" column="0">
- <widget class="QLabel" name="enable_controller_label">
+ <item row="0" column="4">
+ <widget class="QPushButton" name="enable_tuning_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>0</width>
+ <width>60</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
- <width>16777215</width>
+ <width>180</width>
<height>50</height>
</size>
</property>
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
<property name="text">
- <string>Enable</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <string>Tuning</string>
</property>
</widget>
</item>
- <item row="1" column="5">
- <widget class="QPushButton" name="load_yaml_template_button">
+ <item row="0" column="1">
+ <widget class="QPushButton" name="enable_default_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -274,13 +299,19 @@
<height>50</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
<property name="text">
- <string>Template</string>
+ <string>Default</string>
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QPushButton" name="load_yaml_default_button">
+ <item row="0" column="3">
+ <widget class="QPushButton" name="enable_picker_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -299,13 +330,19 @@
<height>50</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
<property name="text">
- <string>Default</string>
+ <string>Picker</string>
</property>
</widget>
</item>
- <item row="0" column="3">
- <widget class="QPushButton" name="enable_picker_button">
+ <item row="0" column="6">
+ <widget class="QPushButton" name="enable_template_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -331,12 +368,25 @@
</font>
</property>
<property name="text">
- <string>Picker</string>
+ <string>Template</string>
</property>
</widget>
</item>
- <item row="0" column="4">
- <widget class="QPushButton" name="enable_tuning_button">
+ <item row="0" 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="0" column="5">
+ <widget class="QPushButton" name="enable_remote_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -356,12 +406,12 @@
</size>
</property>
<property name="text">
- <string>Tuning</string>
+ <string>Remote</string>
</property>
</widget>
</item>
- <item row="1" column="4">
- <widget class="QPushButton" name="load_yaml_tuning_button">
+ <item row="1" column="5">
+ <widget class="QPushButton" name="load_yaml_remote_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -381,7 +431,7 @@
</size>
</property>
<property name="text">
- <string>Tuning</string>
+ <string>Remote</string>
</property>
</widget>
</item>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
index faec75cbad2346e7b65a49e0135356bdec56791e..e6f37426b294f7ef5deda40f757c4c081adcf752 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
@@ -214,6 +214,7 @@
<addaction name="action_showHideController_student"/>
<addaction name="action_showHideController_picker"/>
<addaction name="action_showHideController_tuning"/>
+ <addaction name="action_showHideController_remote"/>
<addaction name="action_showHideController_template"/>
<addaction name="separator"/>
<addaction name="action_testMotors"/>
@@ -320,6 +321,17 @@
<string>TestMotors</string>
</property>
</action>
+ <action name="action_showHideController_remote">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Remote</string>
+ </property>
+ </action>
</widget>
<layoutdefault spacing="6" margin="11"/>
<customwidgets>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/remotecontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/remotecontrollertab.ui
new file mode 100644
index 0000000000000000000000000000000000000000..221c78678dadf65f08ca1bfb44a653476eed6bbe
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/remotecontrollertab.ui
@@ -0,0 +1,1144 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>RemoteControllerTab</class>
+ <widget class="QWidget" name="RemoteControllerTab">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1335</width>
+ <height>937</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="2" column="0">
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="4" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="3" column="0">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="topMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>6</number>
+ </property>
+ <item row="1" column="3">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="1" column="2">
+ <widget class="QPushButton" name="activate_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Activate</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QPushButton" name="subscribe_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Subscribe</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ </layout>
+ </item>
+ <item row="2" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ </layout>
+ </item>
+ <item row="2" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="remote_object_name">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="unsubscribe_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Unsubscribe</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="deactivate_button">
+ <property name="text">
+ <string>Deactivate</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <spacer name="horizontalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="1" column="3">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Setpoint</string>
+ </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="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_5">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QFrame" name="red_frame_position_left">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_measured_title_line2">
+ <property name="text">
+ <string>Position</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="red_frame_position_right">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="5">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="5">
+ <widget class="QLabel" name="label_new_title_line2">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLabel" name="label_row_x">
+ <property name="text">
+ <string>x [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="5">
+ <widget class="QLabel" name="label_new_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>New</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="6">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="3" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_measured_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Measured</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="5">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="label_row_y">
+ <property name="text">
+ <string>y [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLabel" name="label_row_z">
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="5">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLabel" name="label_row_yaw">
+ <property name="text">
+ <string>yaw [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="5">
+ <widget class="QPushButton" name="set_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set New</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <widget class="QLabel" name="label_current_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Current</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QLabel" name="label_row_pitch">
+ <property name="text">
+ <string>pitch [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="5">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="4">
+ <widget class="QLabel" name="label_current_title_line2">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="QLabel" name="label_row_roll">
+ <property name="text">
+ <string>roll [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="7" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_12">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLabel" name="label">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Remote</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="3">
+ <widget class="QLineEdit" name="remote_setpoint_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>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>
+ <item row="5" column="3">
+ <widget class="QLineEdit" name="remote_setpoint_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>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>
+ <item row="6" column="3">
+ <widget class="QLineEdit" name="remote_setpoint_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>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>
+ <item row="3" column="3">
+ <widget class="QFrame" name="red_frame_remote">
+ <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="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 row="7" column="3">
+ <widget class="QLineEdit" name="remote_setpoint_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>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>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_3">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Measured</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Remote</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="2">
+ <widget class="QLineEdit" name="remote_data_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>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>
+ <item row="6" column="2">
+ <widget class="QLineEdit" name="remote_data_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>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>
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="remote_data_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>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>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="remote_data_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>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>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tuningcontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tuningcontrollertab.ui
index 28732632ec93dc1d1dba323e5aa054ac505d9da4..54f3db1f3c52e1a82b3e0e6a70bbb9e722392385 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tuningcontrollertab.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tuningcontrollertab.ui
@@ -860,6 +860,188 @@
</property>
</widget>
</item>
+ <item>
+ <spacer name="horizontalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_A_left">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_23">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Destination A</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_A_right">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_4">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>80</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_B_left">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_24">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Destination B</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_B_right">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:green;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
@@ -878,7 +1060,7 @@
<item row="9" column="1">
<widget class="QLabel" name="label_20">
<property name="text">
- <string>step 2: click toggle button to test the performance</string>
+ <string>step 2: click toggle button to change the destination</string>
</property>
</widget>
</item>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
index eca925ea65b4c4d21022c02a7b2237654aeab443..74f0e5b0c1882a679a3f0691764e062e18de398c 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
@@ -93,6 +93,7 @@ public:
void showHideController_student_changed();
void showHideController_picker_changed();
void showHideController_tuning_changed();
+ void showHideController_remote_changed();
void showHideController_template_changed();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
index ff4cb22e2f14a4a199815371561dbca96b18b902..c48d711f9b009dd51da036b2dae4c56b41890c9b 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
@@ -92,6 +92,7 @@ public:
void showHideController_student_changed();
void showHideController_picker_changed();
void showHideController_tuning_changed();
+ void showHideController_remote_changed();
void showHideController_template_changed();
void testMotors_triggered();
@@ -108,6 +109,7 @@ private slots:
void on_enable_student_button_clicked();
void on_enable_picker_button_clicked();
void on_enable_tuning_button_clicked();
+ void on_enable_remote_button_clicked();
void on_enable_template_button_clicked();
// LOAD YAML BUTTONS ON-CLICK CALLBACK
@@ -115,6 +117,7 @@ private slots:
void on_load_yaml_student_button_clicked();
void on_load_yaml_picker_button_clicked();
void on_load_yaml_tuning_button_clicked();
+ void on_load_yaml_remote_button_clicked();
void on_load_yaml_template_button_clicked();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
index 17da92a8fe72c3fe87f03740c1c5dcb13113a4d4..ed4c43b6c4a052caa3768cf7b46d610c6953370a 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
@@ -131,6 +131,7 @@ private slots:
void on_action_showHideController_student_changed();
void on_action_showHideController_picker_changed();
void on_action_showHideController_tuning_changed();
+ void on_action_showHideController_remote_changed();
void on_action_showHideController_template_changed();
void on_action_testMotors_triggered();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h
new file mode 100644
index 0000000000000000000000000000000000000000..686b250bf9369825a0fff7141f0475c5c461cbc7
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h
@@ -0,0 +1,206 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// The GUI for a Remote Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef REMOTECONTROLLERTAB_H
+#define REMOTECONTROLLERTAB_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QVector>
+#include <QLineEdit>
+#include <QTextStream>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+//#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/ViconSubscribeObjectName.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+
+
+
+namespace Ui {
+class RemoteControllerTab;
+}
+
+class RemoteControllerTab : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit RemoteControllerTab(QWidget *parent = 0);
+ ~RemoteControllerTab();
+
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void poseDataUnavailableSlot();
+
+
+
+private slots:
+ void on_lineEdit_setpoint_new_x_returnPressed();
+ void on_lineEdit_setpoint_new_y_returnPressed();
+ void on_lineEdit_setpoint_new_z_returnPressed();
+ void on_lineEdit_setpoint_new_yaw_returnPressed();
+
+ void on_set_setpoint_button_clicked();
+
+ void on_default_setpoint_button_clicked();
+
+ void on_custom_button_1_clicked();
+ void on_custom_button_2_clicked();
+ void on_custom_button_3_clicked();
+
+
+ // COPIED FROM PREVIOUS REMOTE CONTROLLER GUI
+ void on_subscribe_button_clicked();
+ void on_unsubscribe_button_clicked();
+ void on_activate_button_clicked();
+ void on_deactivate_button_clicked();
+
+
+
+
+
+
+private:
+ Ui::RemoteControllerTab *ui;
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+
+
+#ifdef CATKIN_MAKE
+ // PUBLISHER
+ // > For requesting the setpoint to be changed
+ ros::Publisher requestSetpointChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
+
+ // PUBLISHER
+ // > For notifying that a custom button is pressed
+ ros::Publisher customButtonPublisher;
+
+ // PUBLISHER
+ // > For the Remote Controller subscribe action
+ ros::Publisher remoteSubscribePublisher;
+ // > For the Remote Controller activate action
+ ros::Publisher remoteActivatePublisher;
+
+ // SUBSCRIBER
+ // > For the Remote Controller data
+ ros::Subscriber remoteDataSubscriber;
+ // > For the Remote Control setpoint
+ ros::Subscriber remoteControlSetpointSubscriber;
+
+#endif
+
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+#ifdef CATKIN_MAKE
+
+ // COPIED FROM PREVIOUS REMOTE CONTROLLER GUI
+ // For receiving message with the data of the remote
+ void remoteDataCallback(const dfall_pkg::CrazyflieData& objectData);
+ void remoteControlSetpointCallback(const dfall_pkg::CrazyflieData& setpointData);
+
+
+ // For receiving message that the setpoint was changed
+ void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+
+ // Publish a message when a custom button is pressed
+ void publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer);
+
+ // Fill the header for a message
+ void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
+ void fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+#endif
+
+ void publishSetpoint(float x, float y, float z, float yaw_degrees);
+
+};
+
+#endif // REMOTECONTROLLERTAB_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
index 3ee513c9d267622fcdbf28e69d0aa71c3f2aa3e6..ac62ee9e0272cce08d27414640f317be7999aa90 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
@@ -77,6 +77,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->tuning_controller_tab_widget , &TuningControllerTab::setMeasuredPose
);
+ QObject::connect(
+ this , &ControllerTabs::measuredPoseValueChanged ,
+ ui->remote_controller_tab_widget , &RemoteControllerTab::setMeasuredPose
+ );
+
QObject::connect(
this , &ControllerTabs::measuredPoseValueChanged ,
ui->template_controller_tab_widget , &TemplateControllerTab::setMeasuredPose
@@ -106,6 +111,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->tuning_controller_tab_widget , &TuningControllerTab::poseDataUnavailableSlot
);
+ QObject::connect(
+ this , &ControllerTabs::poseDataUnavailableSignal ,
+ ui->remote_controller_tab_widget , &RemoteControllerTab::poseDataUnavailableSlot
+ );
+
QObject::connect(
this , &ControllerTabs::poseDataUnavailableSignal ,
ui->template_controller_tab_widget , &TemplateControllerTab::poseDataUnavailableSlot
@@ -137,6 +147,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->tuning_controller_tab_widget , &TuningControllerTab::setAgentIDsToCoordinate
);
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->remote_controller_tab_widget , &RemoteControllerTab::setAgentIDsToCoordinate
+ );
+
QObject::connect(
this , &ControllerTabs::agentIDsToCoordinateChanged ,
ui->template_controller_tab_widget , &TemplateControllerTab::setAgentIDsToCoordinate
@@ -247,6 +262,11 @@ void ControllerTabs::showHideController_tuning_changed()
showHideController_toggle("Tuning",ui->tuning_tab);
}
+void ControllerTabs::showHideController_remote_changed()
+{
+ showHideController_toggle("Remote",ui->remote_tab);
+}
+
void ControllerTabs::showHideController_template_changed()
{
showHideController_toggle("Template",ui->template_tab);
@@ -419,7 +439,7 @@ void ControllerTabs::setControllerEnabled(int new_controller)
}
case REMOTE_CONTROLLER:
{
- //ui->controller_enabled_label->setText("Remote");
+ setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->remote_tab );
break;
}
case TUNING_CONTROLLER:
@@ -452,6 +472,7 @@ void ControllerTabs::setAllTabLabelsToNormalColouring()
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->student_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->picker_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->tuning_tab );
+ setTextColourOfTabLabel( m_tab_text_colour_normal , ui->remote_tab );
setTextColourOfTabLabel( m_tab_text_colour_normal , ui->template_tab );
}
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
index 43cc94a3e49af91ce382375bb753b79d4ecfd9aa..6a1e88f17690a7e56dd5701b4a434dee16f2ed98 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
@@ -108,6 +108,12 @@ void EnableControllerLoadYamlBar::showHideController_tuning_changed()
ui->load_yaml_tuning_button->setHidden( !(ui->load_yaml_tuning_button->isHidden()) );
}
+void EnableControllerLoadYamlBar::showHideController_remote_changed()
+{
+ ui->enable_remote_button ->setHidden( !(ui->enable_remote_button->isHidden()) );
+ ui->load_yaml_remote_button->setHidden( !(ui->load_yaml_remote_button->isHidden()) );
+}
+
void EnableControllerLoadYamlBar::showHideController_template_changed()
{
ui->enable_template_button ->setHidden( !(ui->enable_template_button->isHidden()) );
@@ -178,6 +184,17 @@ void EnableControllerLoadYamlBar::on_enable_tuning_button_clicked()
#endif
}
+void EnableControllerLoadYamlBar::on_enable_remote_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_USE_REMOTE_CONTROLLER;
+ this->commandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable Remote Controller");
+#endif
+}
+
void EnableControllerLoadYamlBar::on_enable_template_button_clicked()
{
#ifdef CATKIN_MAKE
@@ -261,6 +278,22 @@ void EnableControllerLoadYamlBar::on_load_yaml_tuning_button_clicked()
#endif
}
+void EnableControllerLoadYamlBar::on_load_yaml_remote_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ dfall_pkg::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "RemoteController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load Remote Controller YAML was clicked.");
+#endif
+}
+
void EnableControllerLoadYamlBar::on_load_yaml_template_button_clicked()
{
#ifdef CATKIN_MAKE
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
index 8129239a5991635ea4d0c574480a214bf879bf39..d6fc815ad0a3377c2e17a98b699c7b2130da6dc1 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
@@ -145,6 +145,8 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ui->action_showHideController_picker->trigger();
// > For the tuning controller
ui->action_showHideController_tuning->trigger();
+ // > For the remote controller
+ ui->action_showHideController_remote->trigger();
// > For the template controller
ui->action_showHideController_template->trigger();
@@ -245,6 +247,13 @@ void MainWindow::on_action_showHideController_tuning_changed()
ui->customWidget_controller_tabs->showHideController_tuning_changed();
}
+void MainWindow::on_action_showHideController_remote_changed()
+{
+ // Notify the UI elements of this change
+ ui->customWidget_enableControllerLoadYamlBar->showHideController_remote_changed();
+ ui->customWidget_controller_tabs->showHideController_remote_changed();
+}
+
void MainWindow::on_action_showHideController_template_changed()
{
// Notify the UI elements of this change
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b4dc5ecf60259e0a9be8cffe732188ef9516dfa9
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp
@@ -0,0 +1,835 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// The GUI for a Remote Controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "remotecontrollertab.h"
+#include "ui_remotecontrollertab.h"
+
+RemoteControllerTab::RemoteControllerTab(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::RemoteControllerTab)
+{
+ ui->setupUi(this);
+
+ // Hide the two red frames that are used to indcated
+ // when pose data is occluded
+ ui->red_frame_position_left->setVisible(false);
+ ui->red_frame_position_right->setVisible(false);
+
+ ui->red_frame_remote->setVisible(false);
+
+ ui->remote_object_name->setText("DFALL_REMOTE01");
+
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[REMOTE CONTROLLER TAB GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[REMOTE CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
+ requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("RemoteControllerService/RequestSetpointChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("RemoteControllerService/SetpointChanged", 1, &RemoteControllerTab::setpointChangedCallback, this);
+ }
+
+ // CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
+ customButtonPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::CustomButtonWithHeader>("RemoteControllerService/CustomButtonPressed", 1);
+
+// // GET THE CURRENT SETPOINT
+// // Only if this is an agent GUI
+// if (m_type == TYPE_AGENT)
+// {
+// // > Request the current setpoint
+// ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<dfall_pkg::GetSetpointService>("RemoteControllerService/GetCurrentSetpoint", false);
+// dfall_pkg::GetSetpointService getSetpointCall;
+// getSetpointCall.request.data = 0;
+// getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+// if(getCurrentSetpointServiceClient.call(getSetpointCall))
+// {
+// setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+// }
+// else
+// {
+// // Inform the user
+// ROS_INFO("[REMOTE CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+// }
+// }
+
+ // REMOTE CONTROLLER PUBLISHERS AND SUBSCRIBERS
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ // > For the Remote Controller subscribe action
+ remoteSubscribePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::ViconSubscribeObjectName>("RemoteControllerService/ViconSubscribeObjectName", 1);
+ // > For the Remote Controller activate action
+ remoteActivatePublisher = nodeHandle_for_this_gui.advertise<std_msgs::Int32>("RemoteControllerService/Activate", 1);
+ // > For the Remote Controller data
+ remoteDataSubscriber = nodeHandle_for_this_gui.subscribe("RemoteControllerService/RemoteData", 1, &RemoteControllerTab::remoteDataCallback, this);;
+ // > For the Remote Controller data
+ remoteControlSetpointSubscriber = nodeHandle_for_this_gui.subscribe("RemoteControllerService/RemoteControlSetpoint", 1, &RemoteControllerTab::remoteControlSetpointCallback, this);
+ }
+
+
+
+
+#endif
+
+}
+
+RemoteControllerTab::~RemoteControllerTab()
+{
+ delete ui;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC U U SSSS TTTTT OOO M M
+// C U U S T O O MM MM
+// C U U SSS T O O M M M
+// C U U S T O O M M
+// CCCC UUU SSSS T OOO M M
+//
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+// COPIED FROM PREVIOUS REMOTE CONTROLLER GUI
+// # Custom buttons for the REMOTE controller service
+void RemoteControllerTab::on_subscribe_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message
+ dfall_pkg::ViconSubscribeObjectName msg;
+ // Set the subscribe flag
+ msg.shouldSubscribe = true;
+ // Set the object name
+ msg.objectName = (ui->remote_object_name->text()).toUtf8().constData();
+ // Publish the message
+ this->remoteSubscribePublisher.publish(msg);
+#endif
+}
+
+void RemoteControllerTab::on_unsubscribe_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message
+ dfall_pkg::ViconSubscribeObjectName msg;
+ // Set the subscribe flag
+ msg.shouldSubscribe = false;
+ // Set the object name
+ msg.objectName = (ui->remote_object_name->text()).toUtf8().constData();
+ // Publish the message
+ this->remoteSubscribePublisher.publish(msg);
+#endif
+}
+
+void RemoteControllerTab::on_activate_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 1;
+ // Publish the message
+ this->remoteActivatePublisher.publish(msg);
+#endif
+}
+
+void RemoteControllerTab::on_deactivate_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 0;
+ // Publish the message
+ this->remoteActivatePublisher.publish(msg);
+#endif
+}
+
+
+#ifdef CATKIN_MAKE
+void RemoteControllerTab::remoteDataCallback(const dfall_pkg::CrazyflieData& objectData)
+{
+ // Check if the object is occluded
+ if (objectData.occluded)
+ {
+
+ // Make visible the red frames to indicate occluded
+ if ( !(ui->red_frame_remote->isVisible()) )
+ ui->red_frame_remote->setVisible(true);
+
+// // Set the column heading label to have a red background
+// // > IMPORTANT: Set the background auto fill property to true
+// ui->remote_data_label->setAutoFillBackground(true);
+// // > Get the pallette currently set for the label
+// QPalette pal = ui->remote_roll_label->palette();
+// // > Set the palette property that will change the background
+// pal.setColor(QPalette::Window, QColor(Qt::red));
+// // > Update the palette for the label
+// ui->remote_data_label->setPalette(pal);
+ }
+ else
+ {
+ // Put the roll, pitch, yaw, and z data into the appropriate fields
+ ui->remote_data_roll ->setText(QString::number( objectData.roll * RAD2DEG, 'f', 1));
+ ui->remote_data_pitch->setText(QString::number( objectData.pitch * RAD2DEG, 'f', 1));
+ ui->remote_data_yaw ->setText(QString::number( objectData.yaw * RAD2DEG, 'f', 1));
+ ui->remote_data_z ->setText(QString::number( objectData.z, 'f', 2));
+
+
+ // Ensure the red frames are not visible
+ if ( ui->red_frame_remote->isVisible() )
+ ui->red_frame_remote->setVisible(false);
+
+// // Set the column heading label to have a "normal" background
+// // > IMPORTANT: Set the background auto fill property to true
+// ui->remote_data_label->setAutoFillBackground(false);
+// // > Get the pallette currently set for the roll label
+// QPalette pal = ui->remote_roll_label->palette();
+// // > Update the palette for the column heading label
+// ui->remote_data_label->setPalette(pal);
+ }
+}
+#endif
+
+
+#ifdef CATKIN_MAKE
+void RemoteControllerTab::remoteControlSetpointCallback(const dfall_pkg::CrazyflieData& setpointData)
+{
+ ui->remote_setpoint_roll ->setText(QString::number( setpointData.roll * RAD2DEG, 'f', 1));
+ ui->remote_setpoint_pitch->setText(QString::number( setpointData.pitch * RAD2DEG, 'f', 1));
+ ui->remote_setpoint_yaw ->setText(QString::number( setpointData.yaw * RAD2DEG, 'f', 1));
+ ui->remote_setpoint_z ->setText(QString::number( setpointData.z, 'f', 2));
+}
+#endif
+
+
+
+
+
+
+
+
+#ifdef CATKIN_MAKE
+void RemoteControllerTab::publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer)
+{
+ // Initialise the message as a local variable
+ dfall_pkg::CustomButtonWithHeader msg;
+ // Fill the header of the message
+ fillCustomButtonMessageHeader( msg );
+ // Fill in the button index
+ msg.button_index = button_index;
+ // Get the line edit data, as a float if possible
+ bool isValidFloat = false;
+ float lineEdit_as_float = (lineEdit_pointer->text()).toFloat(&isValidFloat);
+ // Fill in the data
+ if (isValidFloat)
+ msg.float_data = lineEdit_as_float;
+ else
+ msg.string_data = (lineEdit_pointer->text()).toStdString();
+ // Publish the setpoint
+ this->customButtonPublisher.publish(msg);
+ // Inform the user about the change
+ ROS_INFO_STREAM("[REMOTE CONTROLLER TAB GUI] button " << button_index << " clicked.");
+}
+#endif
+
+
+void RemoteControllerTab::on_custom_button_1_clicked()
+{
+#ifdef CATKIN_MAKE
+ //publish_custom_button_command(1,ui->lineEdit_custom_1);
+#endif
+}
+
+void RemoteControllerTab::on_custom_button_2_clicked()
+{
+#ifdef CATKIN_MAKE
+ //publish_custom_button_command(2,ui->lineEdit_custom_2);
+#endif
+}
+
+void RemoteControllerTab::on_custom_button_3_clicked()
+{
+#ifdef CATKIN_MAKE
+ //publish_custom_button_command(3,ui->lineEdit_custom_3);
+#endif
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// PPPP OOO SSSS EEEEE DDDD A TTTTT A
+// P P O O S E D D A A T A A
+// PPPP O O SSS EEE D D A A T A A
+// P O O S E D D AAAAA T AAAAA
+// P OOO SSSS EEEEE DDDD A A T A A
+// ----------------------------------------------------------------------------------
+
+
+void RemoteControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+{
+ if (!occluded)
+ {
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+ // UPDATE THE MEASUREMENT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_x->setText(qstr + QString::number( x, 'f', 3));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_y->setText(qstr + QString::number( y, 'f', 3));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', 3));
+
+ if (roll < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_roll->setText(qstr + QString::number( roll * RAD2DEG, 'f', 1));
+ if (pitch < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_pitch->setText(qstr + QString::number( pitch * RAD2DEG, 'f', 1));
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+
+ // Ensure the red frames are not visible
+ if ( ui->red_frame_position_left->isVisible() )
+ ui->red_frame_position_left->setVisible(false);
+ if ( ui->red_frame_position_right->isVisible() )
+ ui->red_frame_position_right->setVisible(false);
+ }
+ else
+ {
+ // Make visible the red frames to indicate occluded
+ if ( !(ui->red_frame_position_left->isVisible()) )
+ ui->red_frame_position_left->setVisible(true);
+ if ( !(ui->red_frame_position_right->isVisible()) )
+ ui->red_frame_position_right->setVisible(true);
+ }
+}
+
+
+void RemoteControllerTab::poseDataUnavailableSlot()
+{
+ ui->lineEdit_measured_x->setText("xx.xx");
+ ui->lineEdit_measured_y->setText("xx.xx");
+ ui->lineEdit_measured_z->setText("xx.xx");
+
+ ui->lineEdit_measured_roll->setText("xx.xx");
+ ui->lineEdit_measured_pitch->setText("xx.xx");
+ ui->lineEdit_measured_yaw->setText("xx.xx");
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+//
+// CCCC H H A N N GGGG EEEEE DDDD
+// C H H A A NN N G E D D
+// C HHHHH A A N N N G EEE D D
+// C H H AAAAA N NN G G E D D
+// CCCC H H A A N N GGGG EEEEE DDDD
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void RemoteControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
+{
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+
+ // EXTRACT THE SETPOINT
+ float x = newSetpoint.x;
+ float y = newSetpoint.y;
+ float z = newSetpoint.z;
+ float yaw = newSetpoint.yaw;
+
+ // UPDATE THE SETPOINT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_x->setText(qstr + QString::number( x, 'f', 3));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_y->setText(qstr + QString::number( y, 'f', 3));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_z->setText(qstr + QString::number( z, 'f', 3));
+
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+}
+#endif
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE QQQ U U EEEEE SSSS TTTTT N N EEEEE W W
+// R R E Q Q U U E S T NN N E W W
+// RRRR EEE Q Q U U EEE SSS T N N N EEE W W
+// R R E Q Q U U E S T N NN E W W W
+// R R EEEEE QQ Q UUU EEEEE SSSS T N N EEEEE W W
+//
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+// ----------------------------------------------------------------------------------
+
+
+void RemoteControllerTab::publishSetpoint(float x, float y, float z, float yaw_degrees)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw_degrees * DEG2RAD;
+
+ // Publish the setpoint
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[REMOTE CONTROLLER GUI] Published request for setpoint change to: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[REMOTE CONTROLLER GUI] would publish request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]";
+#endif
+}
+
+
+
+void RemoteControllerTab::on_lineEdit_setpoint_new_x_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void RemoteControllerTab::on_lineEdit_setpoint_new_y_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void RemoteControllerTab::on_lineEdit_setpoint_new_z_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void RemoteControllerTab::on_lineEdit_setpoint_new_yaw_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void RemoteControllerTab::on_set_setpoint_button_clicked()
+{
+
+ // Initialise local variable for each of (x,y,z,yaw)
+ float x = 0.0f, y = 0.0f, z = 0.0f, yaw = 0.0f;
+
+ // Take the new value if available, otherwise use the old value
+ // > For x
+ if(!ui->lineEdit_setpoint_new_x->text().isEmpty())
+ x = (ui->lineEdit_setpoint_new_x->text()).toFloat();
+ else
+ x = (ui->lineEdit_setpoint_current_x->text()).toFloat();
+ // > For y
+ if(!ui->lineEdit_setpoint_new_y->text().isEmpty())
+ y = (ui->lineEdit_setpoint_new_y->text()).toFloat();
+ else
+ y = (ui->lineEdit_setpoint_current_y->text()).toFloat();
+ // > For z
+ if(!ui->lineEdit_setpoint_new_z->text().isEmpty())
+ z = (ui->lineEdit_setpoint_new_z->text()).toFloat();
+ else
+ z = (ui->lineEdit_setpoint_current_z->text()).toFloat();
+ // > For yaw
+ if(!ui->lineEdit_setpoint_new_yaw->text().isEmpty())
+ yaw = (ui->lineEdit_setpoint_new_yaw->text()).toFloat();
+ else
+ yaw = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+ // Call the function to publish the setpoint
+ publishSetpoint(x,y,z,yaw);
+}
+
+void RemoteControllerTab::on_default_setpoint_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Publish this as a blank setpoint with the
+ // "buttonID" field set appropriately
+
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.buttonID = REQUEST_DEFAULT_SETPOINT_BUTTON_ID;
+
+ // Publish the default setpoint button press
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[REMOTE CONTROLLER GUI] Published request for setpoint change to the default");
+#endif
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void RemoteControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+ // // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("RemoteControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[REMOTE CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+
+ // SUBSCRIBERS
+ // > For receiving message that the setpoint was changed
+ setpointChangedSubscriber = agent_base_nodeHandle.subscribe("RemoteControllerService/SetpointChanged", 1, &RemoteControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // Set information back to the default
+ ui->lineEdit_setpoint_current_x->setText("xx.xx");
+ ui->lineEdit_setpoint_current_y->setText("xx.xx");
+ ui->lineEdit_setpoint_current_z->setText("xx.xx");
+ ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
+
+ }
+#endif
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
+// MM MM S G G H H E A A D D E R R
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void RemoteControllerTab::fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[REMOTE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void RemoteControllerTab::fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[REMOTE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool RemoteControllerTab::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the value of the "type" parameter into a local string variable
+ std::string type_string;
+ if(!nodeHandle.getParam("type", type_string))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[REMOTE CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[REMOTE CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[REMOTE CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[REMOTE CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[REMOTE CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[REMOTE CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[REMOTE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
index ad36c34dbcdae0489361f3ebc544b6f57cf085ec..6c1bf28f4abaa2a48a0923622eb95a70cf6769c5 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
@@ -55,6 +55,12 @@ TuningControllerTab::TuningControllerTab(QWidget *parent) :
ui->lineEdit_setpoint->setText( QString::number( m_current_setpoint, 'f', DECIMAL_PLACES_SETPOINT) );
+ ui->frame_A_left->setVisible(false);
+ ui->frame_A_right->setVisible(false);
+ ui->frame_B_left->setVisible(false);
+ ui->frame_B_right->setVisible(false);
+
+
#ifdef CATKIN_MAKE
//ros::init();
@@ -324,13 +330,37 @@ void TuningControllerTab::on_button_setpoint_toggle_clicked()
float new_setpoint = 0.0;
if (setpoint_copy < SETPOINT_X_MINUS)
+ {
new_setpoint = SETPOINT_X_MINUS;
+ ui->frame_A_left->setVisible(true);
+ ui->frame_A_right->setVisible(true);
+ ui->frame_B_left->setVisible(false);
+ ui->frame_B_right->setVisible(false);
+ }
else if (setpoint_copy < 0.0)
+ {
new_setpoint = SETPOINT_X_PLUS;
+ ui->frame_A_left->setVisible(false);
+ ui->frame_A_right->setVisible(false);
+ ui->frame_B_left->setVisible(true);
+ ui->frame_B_right->setVisible(true);
+ }
else if (setpoint_copy > SETPOINT_X_PLUS)
+ {
new_setpoint = SETPOINT_X_PLUS;
+ ui->frame_A_left->setVisible(false);
+ ui->frame_A_right->setVisible(false);
+ ui->frame_B_left->setVisible(true);
+ ui->frame_B_right->setVisible(true);
+ }
else
+ {
new_setpoint = SETPOINT_X_MINUS;
+ ui->frame_A_left->setVisible(true);
+ ui->frame_A_right->setVisible(true);
+ ui->frame_B_left->setVisible(false);
+ ui->frame_B_right->setVisible(false);
+ }
ui->lineEdit_setpoint->setText( QString::number( new_setpoint, 'f', DECIMAL_PLACES_SETPOINT) );
float lineEdit_as_float_rounded = (ui->lineEdit_setpoint->text()).toFloat();
diff --git a/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
index f3f96349c1ddc4a27997d2d72b9264e6076447b4..4174f8f453d5d4c22f40c0916735982e6b407f06 100644
--- a/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
+++ b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
@@ -115,6 +115,8 @@ void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std:
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
+
// FUNCTIONS FOR GETTING IDs AND NAMESPACES
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index 3f49b46f9abfc2cd7ccfad634ef8a204f34a4cf7..b41dee020f20f6f27ca04ab41d46919cc14b35ab 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -204,6 +204,8 @@ ros::ServiceClient m_studentController;
ros::ServiceClient m_tuningController;
// The Picker controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_pickerController;
+// The Remote controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_remoteController;
// The Template controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_templateController;
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
index 968f78ec66df2c2b79f45ac7d4a8d86749aeab37..2b9e5899cd5a85fdc4a980e49f914976b71d0c7b 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
@@ -25,7 +25,7 @@
//
//
// DESCRIPTION:
-// Place for students to implement their controller
+// Remote Controller
//
// ----------------------------------------------------------------------------------
@@ -55,19 +55,46 @@
#include "ros/ros.h"
#include <ros/package.h>
-//the generated structs from the msg-files have to be included
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
#include "dfall_pkg/ViconData.h"
#include "dfall_pkg/Setpoint.h"
#include "dfall_pkg/ControlCommand.h"
#include "dfall_pkg/Controller.h"
#include "dfall_pkg/DebugMsg.h"
-#include "dfall_pkg/CustomButton.h"
#include "dfall_pkg/ViconSubscribeObjectName.h"
-// Include the Parameter Service shared definitions
+// Include the DFALL service types
+#include "dfall_pkg/IntIntService.h"
+#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/GetSetpointService.h"
+#include "dfall_pkg/CMQuery.h"
+
+// Include the shared definitions
#include "nodes/Constants.h"
+#include "nodes/DefaultControllerConstants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
-#include <std_msgs/Int32.h>
@@ -97,9 +124,9 @@
// body frame roll, pitch, and yaw angles from the PID attitude
// controllers implemented in the Crazyflie 2.0 firmware.
-#define CF_COMMAND_TYPE_MOTOR 6
-#define CF_COMMAND_TYPE_RATE 7
-#define CF_COMMAND_TYPE_ANGLE 8
+// #define CF_COMMAND_TYPE_MOTOR 6
+// #define CF_COMMAND_TYPE_RATE 7
+// #define CF_COMMAND_TYPE_ANGLE 8
// These constants define the controller used for computing the response in the
@@ -159,11 +186,11 @@
// LQR_ANGLE_MODE LQR controller based on the state vector:
// [position,velocity]
//
-#define LQR_RATE_MODE 1 // (DEFAULT)
-#define LQR_ANGLE_MODE 2
+// #define LQR_RATE_MODE 1 // (DEFAULT)
+// #define LQR_ANGLE_MODE 2
+
+
-// Namespacing the package
-using namespace dfall_pkg;
// ----------------------------------------------------------------------------------
@@ -223,9 +250,9 @@ float remoteConrtolSetpointFactor_yaw = 1.0;
float remoteConrtolSetpointFactor_z = 1.0;
// LQR Gain matrix for tracking the angle commands from the Crazyflie
-std::vector<float> gainMatrixRollRate_forRemoteControl (3,0.0);
-std::vector<float> gainMatrixPitchRate_forRemoteControl (3,0.0);
-std::vector<float> gainMatrixYawRate_forRemoteControl (3,0.0);
+std::vector<float> gainMatrixRollRate_forRemoteControl = {5.00, 0.00, 0.00};
+std::vector<float> gainMatrixPitchRate_forRemoteControl = {0.00, 5.00, 0.00};
+std::vector<float> gainMatrixYawRate_forRemoteControl = {0.00, 0.00, 2.80};
// ******************************************************************************* //
@@ -233,13 +260,13 @@ std::vector<float> gainMatrixYawRate_forRemoteControl (3,0.0);
// VARIABLES FOR SOME "ALMOST CONSTANTS"
// > Mass of the Crazyflie quad-rotor, in [grams]
-float cf_mass;
+float cf_mass = 25.0;
// > Coefficients of the 16-bit command to thrust conversion
-std::vector<float> motorPoly(3);
+std::vector<float> motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
// The weight of the Crazyflie in [Newtons], i.e., mg
-float gravity_force;
+float gravity_force = cf_mass * 9.81 / 1000.0;;
// One quarter of the "gravity_force"
-float gravity_force_quarter;
+float gravity_force_quarter = 0.25 * gravity_force;
@@ -247,10 +274,10 @@ float gravity_force_quarter;
// VARIABLES FOR THE CONTROLLER
// Frequency at which the controller is running
-float vicon_frequency;
+float vicon_frequency = 200.0;
// Frequency at which the controller is running
-float control_frequency;
+float control_frequency = 200.0;
// > The setpoints for (x,y,z) position and yaw angle, in that order
@@ -263,24 +290,24 @@ float setpoint[4] = {0.0,0.0,0.4,0.0};
int controller_mode = CONTROLLER_MODE_LQR_RATE;
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
-std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
-std::vector<float> gainMatrixRollRate (9,0.0);
-std::vector<float> gainMatrixPitchRate (9,0.0);
-std::vector<float> gainMatrixYawRate (9,0.0);
+std::vector<float> gainMatrixThrust_NineStateVector = {0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00};
+std::vector<float> gainMatrixRollRate = {0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00};
+std::vector<float> gainMatrixPitchRate = {1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00};
+std::vector<float> gainMatrixYawRate = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE"
-std::vector<float> gainMatrixThrust_SixStateVector (6,0.0);
-std::vector<float> gainMatrixRollAngle (6,0.0);
-std::vector<float> gainMatrixPitchAngle (6,0.0);
+std::vector<float> gainMatrixThrust_SixStateVector = {0.00, 0.00, 0.98, 0.00, 0.00, 0.25};
+std::vector<float> gainMatrixRollAngle = {0.00,-0.20, 0.00, 0.00,-0.20, 0.00};
+std::vector<float> gainMatrixPitchAngle = {0.20, 0.00, 0.00, 0.20, 0.00, 0.00};
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED"
-std::vector<float> gainMatrixThrust_SixStateVector_50Hz (6,0.0);
-std::vector<float> gainMatrixRollAngle_50Hz (6,0.0);
-std::vector<float> gainMatrixPitchAngle_50Hz (6,0.0);
+std::vector<float> gainMatrixThrust_SixStateVector_50Hz = {0.00, 0.00, 0.82, 0.00, 0.00, 0.22};
+std::vector<float> gainMatrixRollAngle_50Hz = {0.00,-0.31, 0.00, 0.00,-0.25, 0.00};
+std::vector<float> gainMatrixPitchAngle_50Hz = {0.31, 0.00, 0.00, 0.25, 0.00, 0.00};
-std::vector<float> gainMatrixRollRate_Nested (3,0.0);
-std::vector<float> gainMatrixPitchRate_Nested (3,0.0);
-std::vector<float> gainMatrixYawRate_Nested (3,0.0);
+std::vector<float> gainMatrixRollRate_Nested = {4.00, 0.00, 0.00};
+std::vector<float> gainMatrixPitchRate_Nested = {0.00, 4.00, 0.00};
+std::vector<float> gainMatrixYawRate_Nested = {0.00, 0.00, 2.30};
int lqr_angleRateNested_counter = 4;
float lqr_angleRateNested_prev_thrustAdjustment = 0.0;
@@ -290,14 +317,14 @@ float lqr_angleRateNested_prev_yawAngle = 0.0;
// The 16-bit command limits
-float cmd_sixteenbit_min;
-float cmd_sixteenbit_max;
+float cmd_sixteenbit_min = 1000;
+float cmd_sixteenbit_max = 60000;
// VARIABLES FOR THE ESTIMATOR
// Frequency at which the controller is running
-float estimator_frequency;
+float estimator_frequency = 200.0;
// > A flag for which estimator to use:
int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
@@ -323,21 +350,21 @@ float stateInterialEstimate_viaPointMassKalmanFilter[12];
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
-std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
-std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
-std::vector<float> PMKF_Kinf_for_positions (2,0.0);
+std::vector<float> PMKF_Ahat_row1_for_positions = { 0.6723, 0.0034};
+std::vector<float> PMKF_Ahat_row2_for_positions = {-12.9648, 0.9352};
+std::vector<float> PMKF_Kinf_for_positions = { 0.3277,12.9648};
// > For the (roll,pitch,yaw) angles
-std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
-std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
-std::vector<float> PMKF_Kinf_for_angles (2,0.0);
+std::vector<float> PMKF_Ahat_row1_for_angles = { 0.6954, 0.0035};
+std::vector<float> PMKF_Ahat_row2_for_angles = {-11.0342, 0.9448};
+std::vector<float> PMKF_Kinf_for_angles = { 0.3046,11.0342};
// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
// > For the paramter service of this agent
-std::string namespace_to_own_agent_parameter_service;
+std::string m_namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
-std::string namespace_to_coordinator_parameter_service;
+std::string m_namespace_to_coordinator_parameter_service;
// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
@@ -359,11 +386,14 @@ bool shouldPublishDebugMessage = false;
bool shouldDisplayDebugInfo = false;
-// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
-// POSITION
+// ------------------------------------------------------
+// VARIABLES THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
-// The ID of this agent, i.e., the ID of this compute
-int my_agentID = 0;
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
@@ -443,18 +473,12 @@ void setpointCallback(const Setpoint& newSetpoint);
-// LOAD PARAMETERS
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
-int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
-bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
-std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-void fetchYamlParameters(ros::NodeHandle& nodeHandle);
-void processFetchedParameters();
+// LOADING OF YAML PARAMETERS
+void timerCallback_initial_load_yaml(const ros::TimerEvent&);
+void isReadyRemoteControllerYamlCallback(const IntWithHeader & msg);
+void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle);
+
diff --git a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
index 29ec896efe0bd6f6cc94b0bdf52ba950467aefa8..e6bacb8f6af079883950dd1df95c9b4a49056ca3 100755
--- a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
@@ -4,6 +4,7 @@ defaultController: "DefaultControllerService/DefaultController"
studentController: "StudentControllerService/StudentController"
tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
+remoteController: "RemoteControllerService/RemoteController"
templateController: "TemplateControllerService/TemplateController"
testMotorsController: "TestMotorsControllerService/TestMotorsController"
diff --git a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
index 2d48a2214683291495c54ce62bfa20188d130f44..c0865950e3f37775a4998a69faf505e1a7942634 100644
--- a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
@@ -74,6 +74,17 @@
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
+{
+ std::string val;
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
{
float val;
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 769deba2cf9936a5d1b9fe19db00a7aafa431099..9bd6791af148892c0035182b56f5e76cfa6c84b3 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -838,6 +838,9 @@ void setInstantController(int controller)
case PICKER_CONTROLLER:
m_instant_controller_service_client = &m_pickerController;
break;
+ case REMOTE_CONTROLLER:
+ m_instant_controller_service_client = &m_remoteController;
+ break;
case TEMPLATE_CONTROLLER:
m_instant_controller_service_client = &m_templateController;
break;
@@ -1272,6 +1275,7 @@ void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
createControllerServiceClientFromParameterName( "studentController" , m_studentController );
createControllerServiceClientFromParameterName( "tuningController" , m_tuningController );
createControllerServiceClientFromParameterName( "pickerController" , m_pickerController );
+ createControllerServiceClientFromParameterName( "remoteController" , m_remoteController );
createControllerServiceClientFromParameterName( "templateController" , m_templateController );
createControllerServiceClientFromParameterName( "testMotorsController" , m_testMotorsController );
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
index de85865dde6359d0ca47fa5c66a4292a42983054..b301d3fc9b5795907e4055dd5edac0b60cb87f88 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
@@ -25,7 +25,7 @@
//
//
// DESCRIPTION:
-// Place for students to implement their controller
+// Remote Controller
//
// ----------------------------------------------------------------------------------
@@ -247,11 +247,29 @@ void calculateControlOutput_forRemoteControl(float stateInertial[12], Controller
float yawRate_forResponse = 0;
float thrustAdjustment_total = 0;
+ // Fill in the yaw angle error
+ // > This error should be "unwrapped" to be in the range
+ // ( -pi , pi )
+ // > First, get the yaw error into a local variable
+ float yawError = stateInertial[8] - setpointFromRemote_yaw;
+ // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
+ while(yawError > PI) {yawError -= 2 * PI;}
+ while(yawError < -PI) {yawError += 2 * PI;}
+ // > Third, clip the "yawError"
+ if (yawError>(PI/6))
+ {
+ yawError = (PI/6);
+ }
+ else if (yawError<(-PI/6))
+ {
+ yawError = (-PI/6);
+ }
+
// Create the angle error to use for the inner controller
float temp_stateAngleError[3] = {
stateInertial[6] - setpointFromRemote_roll,
stateInertial[7] - setpointFromRemote_pitch,
- stateInertial[8] - setpointFromRemote_yaw
+ yawError
};
// Create the z-error
@@ -294,7 +312,7 @@ void calculateControlOutput_forRemoteControl(float stateInertial[12], Controller
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -322,10 +340,13 @@ void viconSubscribeObjectNameCallback(const ViconSubscribeObjectName& msg)
// Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
// the "~" indcates that "self" is the node handle assigned to this variable.
- ros::NodeHandle nodeHandle("~");
+ //ros::NodeHandle nodeHandle("~");
+
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
// Keeps 10 messages because otherwise ViconDataPublisher would override the data immediately
- viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 10, viconCallback);
+ viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 10, viconCallback);
ROS_INFO_STREAM("[REMOTE CONTORLLER] successfully subscribed to ViconData");
}
else
@@ -434,7 +455,7 @@ void shouldActivateCallback(const std_msgs::Int32& msg)
setpointFromRemote_roll = 0.0;
setpointFromRemote_pitch = 0.0;
setpointFromRemote_yaw = 0.0;
- setpointFromRemote_z = 0.0;
+ setpointFromRemote_z = 0.2;
z_when_remote_activated = 0.0;
}
@@ -640,7 +661,7 @@ void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request
response.controlOutput.motorCmd2 = 0;
response.controlOutput.motorCmd3 = 0;
response.controlOutput.motorCmd4 = 0;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
break;
}
}
@@ -699,7 +720,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -785,7 +806,7 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -890,7 +911,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
// response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
@@ -1210,90 +1231,120 @@ void setpointCallback(const Setpoint& newSetpoint)
// P A A R R A A M M EEEEE T EEEEE R R SSSS
// ----------------------------------------------------------------------------------
+// TIMER CALLBACK FOR SENDING THE LOAD YAML REQUEST
+// This function does NOT need to be edited
+void timerCallback_initial_load_yaml(const ros::TimerEvent&)
+{
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "RemoteController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyRemoteControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[REMOTE CONTROLLER] The request load yaml file service call failed.");
+ }
+}
-// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+
+// LOADING OF YAML PARAMETERS
+void isReadyRemoteControllerYamlCallback(const IntWithHeader & msg)
{
- // // Extract from the "msg" for which controller the and from where to fetch the YAML
- // // parameters
- // int controller_to_fetch_yaml = msg.data;
-
- // // Switch between fetching for the different controllers and from different locations
- // switch(controller_to_fetch_yaml)
- // {
-
- // // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- // case FETCH_YAML_REMOTE_CONTROLLER_FROM_OWN_AGENT:
- // {
- // // Let the user know that this message was received
- // ROS_INFO("[REMOTE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
- // // Create a node handle to the parameter service running on this agent's machine
- // ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
- // // Call the function that fetches the parameters
- // fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
- // break;
- // }
-
- // // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- // case FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR:
- // {
- // // Let the user know that this message was received
- // ROS_INFO("[REMOTE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
- // // Create a node handle to the parameter service running on the coordinator machine
- // ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
- // // Call the function that fetches the parameters
- // fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
- // break;
- // }
-
- // default:
- // {
- // // Let the user know that the command was not relevant
- // //ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
- // //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
- // break;
- // }
- // }
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[REMOTE CONTROLLER] Now fetching the RemoteController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[REMOTE CONTROLLER] Now fetching the RemoteController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[REMOTE CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchRemoteControllerYamlParameters(nodeHandle_to_use);
+ }
}
// This function CAN BE edited for successful completion of the classroom exercise, and the
// use of parameters fetched from the YAML file is highly recommended to make tuning of
// your controller easier and quicker.
-void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
{
// Here we load the parameters that are specified in the CustomController.yaml file
// Add the "CustomController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_RemoteController(nodeHandle, "RemoteController");
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "RemoteController");
// ******************************************************************************* //
// PARAMETERS SPECIFIC TO THE REMOTE CONTROL FEATURE
// Vicon object name of the Remote to follow
- default_viconObjectName_forRemote = getParameterString(nodeHandle_for_RemoteController, "default_viconObjectName_forRemote");
+ default_viconObjectName_forRemote = getParameterString(nodeHandle_for_paramaters, "default_viconObjectName_forRemote");
// Boolean for whether the Remote's state should be published as a message
- shouldPublishRemote_xyz_rpy = getParameterBool(nodeHandle_for_RemoteController, "shouldPublishRemote_xyz_rpy");
+ shouldPublishRemote_xyz_rpy = getParameterBool(nodeHandle_for_paramaters, "shouldPublishRemote_xyz_rpy");
// Boolean for whether the Remote's state should be display in the terminal window
- shouldDisplayRemote_xyz_rpy = getParameterBool(nodeHandle_for_RemoteController, "shouldDisplayRemote_xyz_rpy");
+ shouldDisplayRemote_xyz_rpy = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayRemote_xyz_rpy");
// Roll and pitch limit (in degrees for angles)
- remoteControlLimit_roll_degrees = getParameterFloat(nodeHandle_for_RemoteController , "remoteControlLimit_roll_degrees");
- remoteControlLimit_pitch_degrees = getParameterFloat(nodeHandle_for_RemoteController , "remoteControlLimit_pitch_degrees");
+ remoteControlLimit_roll_degrees = getParameterFloat(nodeHandle_for_paramaters , "remoteControlLimit_roll_degrees");
+ remoteControlLimit_pitch_degrees = getParameterFloat(nodeHandle_for_paramaters , "remoteControlLimit_pitch_degrees");
// Factor by which to reduce the remote control input
- remoteConrtolSetpointFactor_roll = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_roll");
- remoteConrtolSetpointFactor_pitch = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_pitch");
- remoteConrtolSetpointFactor_yaw = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_yaw");
- remoteConrtolSetpointFactor_z = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_z");
+ remoteConrtolSetpointFactor_roll = getParameterFloat(nodeHandle_for_paramaters , "remoteConrtolSetpointFactor_roll");
+ remoteConrtolSetpointFactor_pitch = getParameterFloat(nodeHandle_for_paramaters , "remoteConrtolSetpointFactor_pitch");
+ remoteConrtolSetpointFactor_yaw = getParameterFloat(nodeHandle_for_paramaters , "remoteConrtolSetpointFactor_yaw");
+ remoteConrtolSetpointFactor_z = getParameterFloat(nodeHandle_for_paramaters , "remoteConrtolSetpointFactor_z");
// LQR Gain matrix for tracking the angle commands from the Crazyflie
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollRate_forRemoteControl", gainMatrixRollRate_forRemoteControl, 3);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchRate_forRemoteControl", gainMatrixPitchRate_forRemoteControl, 3);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixYawRate_forRemoteControl", gainMatrixYawRate_forRemoteControl, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_forRemoteControl", gainMatrixRollRate_forRemoteControl, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_forRemoteControl", gainMatrixPitchRate_forRemoteControl, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate_forRemoteControl", gainMatrixYawRate_forRemoteControl, 3);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/default_viconObjectName_forRemote = " << default_viconObjectName_forRemote);
@@ -1303,97 +1354,88 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > The mass of the crazyflie
- cf_mass = getParameterFloat(nodeHandle_for_RemoteController , "mass");
+ cf_mass = getParameterFloat(nodeHandle_for_paramaters , "mass");
// Display one of the YAML parameters to debug if it is working correctly
//ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- vicon_frequency = getParameterFloat(nodeHandle_for_RemoteController, "vicon_frequency");
+ vicon_frequency = getParameterFloat(nodeHandle_for_paramaters, "vicon_frequency");
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- control_frequency = getParameterFloat(nodeHandle_for_RemoteController, "control_frequency");
+ control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_RemoteController, "motorPoly", motorPoly, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
- shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_RemoteController, "shouldPerformConvertIntoBodyFrame");
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paramaters, "shouldPerformConvertIntoBodyFrame");
// Boolean indiciating whether the "Debug Message" of this agent should be published or not
- shouldPublishDebugMessage = getParameterBool(nodeHandle_for_RemoteController, "shouldPublishDebugMessage");
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paramaters, "shouldPublishDebugMessage");
// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
- shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_RemoteController, "shouldDisplayDebugInfo");
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
// THE CONTROLLER GAINS:
// A flag for which controller to use:
- controller_mode = getParameterInt( nodeHandle_for_RemoteController , "controller_mode" );
+ controller_mode = getParameterInt( nodeHandle_for_paramaters , "controller_mode" );
// A flag for which estimator to use:
- estimator_method = getParameterInt( nodeHandle_for_RemoteController , "estimator_method" );
+ estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// 16-BIT COMMAND LIMITS
- cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_RemoteController, "command_sixteenbit_min");
- cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_RemoteController, "command_sixteenbit_max");
+ cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+ cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_max");
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
//ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/mass = " << cf_mass);
- // Call the function that computes details an values that are needed from these
- // parameters loaded above
- processFetchedParameters();
-}
+ // PROCESS THE PARAMTERS
-
-// This function CAN BE edited for successful completion of the classroom exercise, and the
-// use of parameters loaded from the YAML file is highly recommended to make tuning of
-// your controller easier and quicker.
-void processFetchedParameters()
-{
// Compute the feed-forward force that we need to counteract gravity (i.e., mg)
// > in units of [Newtons]
- gravity_force = cf_mass * 9.81/(1000);
- gravity_force_quarter = cf_mass * 9.81/(1000*4);
+ gravity_force = cf_mass * 9.81/(1000.0);
+ gravity_force_quarter = cf_mass * 9.81/(1000.0*4.0);
// Set that the estimator frequency is the same as the control frequency
estimator_frequency = vicon_frequency;
@@ -1417,83 +1459,6 @@ void processFetchedParameters()
-// ----------------------------------------------------------------------------------
-// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
-// G E T P P A A R R A A MM MM ( )
-// G EEE T PPPP A A RRRR A A M M M ( )
-// G G E T P AAAAA R R AAAAA M M ( )
-// GGGG EEEEE T P A A R R A A M M ( )
-// ----------------------------------------------------------------------------------
-
-
-// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
-{
- float val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
-{
- int val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
-{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val.size();
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
-{
- bool val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
-{
- std::string val;
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
-
@@ -1506,101 +1471,128 @@ std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
// M M A A III N N
// ----------------------------------------------------------------------------------
-// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
int main(int argc, char* argv[]) {
// Starting the ROS-node
ros::init(argc, argv, "RemoteControllerService");
- // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
- // the "~" indcates that "self" is the node handle assigned to this variable.
+ // Create a "ros::NodeHandle" type local variable "nodeHandle"
+ // as the current node, the "~" indcates that "self" is the
+ // node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
- // Get the namespace of this "StudentControllerService" node
- // > This should be something like: "/dfall/agentXXX"
+ // Get the namespace of this "RemoteControllerService" node
std::string m_namespace = ros::this_node::getNamespace();
ROS_INFO_STREAM("[REMOTE CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
- // Get the agent ID as the "ROS_NAMESPACE" this computer.
- // NOTES:
- // > If you look at the "Student.launch" file in the "launch" folder, you will see
- // the following line of code:
- // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
- // This line of code adds a parameter named "studentID" to the "FlyingAgentClient"
- // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
- // to the "FlyingAgentClient" node within which this controller service is nested.
- // Get the handle to the "FlyingAgentClient" node
- ros::NodeHandle FlyingAgentClient_nodeHandle(m_namespace + "/FlyingAgentClient");
- // Get the value of the "studentID" parameter into the instance variable "my_agentID"
- if(!FlyingAgentClient_nodeHandle.getParam("agentID", my_agentID))
- {
- // Throw an error if the student ID parameter could not be obtained
- ROS_ERROR("[REMOTE CONTROLLER] Failed to get agentID from FlyingAgentClient");
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[REMOTE CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
}
- // *********************************************************************************
- // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
- // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
- // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
- // namespace string for the parameter service that is running on the machine of this
- // agent
- namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
- // Create a node handle to the parameter service running on this agent's machine
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
- // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
- // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
- // and calls the class function "yamlReadyForFetchCallback" each time a message is
- // received on this topic and the message is passed as an input argument to the
- // "yamlReadyForFetchCallback" class function.
- ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
- // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
- // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
- // for the parameter service that is running on the coordinate machine
- // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
- // is very important because it specifies that the name is global
- namespace_to_coordinator_parameter_service = "/ParameterService";
- // Create a node handle to the parameter service running on the coordinator machine
- ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
- //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
-
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber remoteContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "RemoteController", 1, isReadyRemoteControllerYamlCallback);
+ ros::Subscriber remoteContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("RemoteController", 1, isReadyRemoteControllerYamlCallback);
+
+
- // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
- // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
- // and calls the class function "yamlReadyForFetchCallback" each time a message is
- // received on this topic and the message is passed as an input argument to the
- // "yamlReadyForFetchCallback" class function.
- ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
- //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+ // This can be done either here or as part of declaring the
+ // variables in the header file
- // PRINT OUT SOME INFORMATION
- // Let the user know what namespaces are being used for linking to the parameter service
- ROS_INFO_STREAM("[REMOTE CONTROLLER] The namespace string for accessing the Paramter Services are:");
- ROS_INFO_STREAM("[REMOTE CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
- ROS_INFO_STREAM("[REMOTE CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a service client
+ // we stall the availability of this node until the
+ // paramter service is ready
+ // > NOTE FURTHER that calling on the service directly from here
+ // often means the yaml file is not actually loaded. So we
+ // instead use a timer to delay the loading
- // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+ // Create a single-shot timer
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(2.5), timerCallback_initial_load_yaml, true);
+ timer_initial_load_yaml.start();
- // Call the class function that loads the parameters for this class.
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
- // *********************************************************************************
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+
+ // SPECIFIC TO THE REMOTE CONTORLLER
//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
//ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 10, viconCallback);
//ROS_INFO_STREAM("[REMOTE CONTORLLER] successfully subscribed to ViconData");
@@ -1620,9 +1612,13 @@ int main(int argc, char* argv[]) {
- // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
- // advertises under the name "DebugTopic" and is a message with the structure
- // defined in the file "DebugMsg.msg" (located in the "msg" folder).
+
+
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
// Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"