diff --git a/dfall_ws/src/dfall_pkg/CMakeLists.txt b/dfall_ws/src/dfall_pkg/CMakeLists.txt
index c8aa8c4bc2067ce13485752083e57e9d973d5742..c93e0b494d5e62a72ee436972814933801e93c25 100755
--- a/dfall_ws/src/dfall_pkg/CMakeLists.txt
+++ b/dfall_ws/src/dfall_pkg/CMakeLists.txt
@@ -166,6 +166,7 @@ if(Qt5_FOUND)
${FLYING_AGENT_GUI_LIB_PATH_INC}/controllertabs.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/coordinator.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/coordinatorrow.h
+ ${FLYING_AGENT_GUI_LIB_PATH_INC}/csonecontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/defaultcontrollertab.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/enablecontrollerloadyamlbar.h
${FLYING_AGENT_GUI_LIB_PATH_INC}/mainwindow.h
@@ -184,6 +185,7 @@ if(Qt5_FOUND)
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/controllertabs.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/coordinator.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/coordinatorrow.ui
+ ${FLYING_AGENT_GUI_LIB_PATH_FORMS}/csonecontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/defaultcontrollertab.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/enablecontrollerloadyamlbar.ui
${FLYING_AGENT_GUI_LIB_PATH_FORMS}/mainwindow.ui
@@ -390,6 +392,8 @@ add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(CrazyRadioEmulator src/nodes/CrazyRadioEmulator.cpp
src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(CsoneControllerService src/nodes/CsoneControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
add_executable(DefaultControllerService src/nodes/DefaultControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
@@ -449,6 +453,7 @@ if(Qt5_FOUND)
${FLYING_AGENT_GUI_LIB_PATH_SRC}/controllertabs.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/coordinator.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/coordinatorrow.cpp
+ ${FLYING_AGENT_GUI_LIB_PATH_SRC}/csonecontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/defaultcontrollertab.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/enablecontrollerloadyamlbar.cpp
${FLYING_AGENT_GUI_LIB_PATH_SRC}/pickercontrollertab.cpp
@@ -482,6 +487,7 @@ endif()
add_dependencies(FlyingAgentClient dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(BatteryMonitor dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CrazyRadioEmulator dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(CsoneControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(DefaultControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(SafeControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(DemoControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -535,6 +541,7 @@ endif()
target_link_libraries(FlyingAgentClient ${catkin_LIBRARIES})
target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
target_link_libraries(CrazyRadioEmulator ${catkin_LIBRARIES})
+target_link_libraries(CsoneControllerService ${catkin_LIBRARIES})
target_link_libraries(DefaultControllerService ${catkin_LIBRARIES})
target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
index daf227898c1ca5ea81aa9303b06e6dee3b202c71..a479b1b1f0713b9f2391984f5165f96655190a18 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/flyingAgentGUI.pro
@@ -22,6 +22,7 @@ SOURCES += src/main.cpp\
src/connectstartstopbar.cpp \
src/enablecontrollerloadyamlbar.cpp \
src/controllertabs.cpp \
+ src/csonecontrollertab.cpp \
src/safecontrollertab.cpp \
src/coordinator.cpp \
src/coordinatorrow.cpp \
@@ -38,6 +39,7 @@ HEADERS += include/mainwindow.h \
include/connectstartstopbar.h \
include/enablecontrollerloadyamlbar.h \
include/controllertabs.h \
+ include/csonecontrollertab.h \
include/safecontrollertab.h \
include/coordinator.h \
include/coordinatorrow.h \
@@ -56,6 +58,7 @@ FORMS += forms/mainwindow.ui \
forms/connectstartstopbar.ui \
forms/enablecontrollerloadyamlbar.ui \
forms/controllertabs.ui \
+ forms/csonecontrollertab.ui \
forms/safecontrollertab.ui \
forms/coordinator.ui \
forms/coordinatorrow.ui \
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/controllertabs.ui
index ceeabd07c08162f564703ebf09e27935f1c85ae4..fc801c9cce0773661c6546d9648edadaecc4c838 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
@@ -28,7 +28,7 @@
<item row="0" column="0">
<widget class="QTabWidget" name="controller_tabs_widget">
<property name="currentIndex">
- <number>4</number>
+ <number>2</number>
</property>
<property name="movable">
<bool>true</bool>
@@ -53,6 +53,16 @@
</item>
</layout>
</widget>
+ <widget class="QWidget" name="csone_tab">
+ <attribute name="title">
+ <string>CS1</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_8">
+ <item row="0" column="0">
+ <widget class="CsoneControllerTab" name="csone_controller_tab_widget" native="true"/>
+ </item>
+ </layout>
+ </widget>
<widget class="QWidget" name="picker_tab">
<attribute name="title">
<string>Picker</string>
@@ -134,6 +144,12 @@
<header>remotecontrollertab.h</header>
<container>1</container>
</customwidget>
+ <customwidget>
+ <class>CsoneControllerTab</class>
+ <extends>QWidget</extends>
+ <header>csonecontrollertab.h</header>
+ <container>1</container>
+ </customwidget>
</customwidgets>
<resources/>
<connections/>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
new file mode 100644
index 0000000000000000000000000000000000000000..3d1882471f2677b96250d14bb69d46c022fe1a24
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
@@ -0,0 +1,872 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>CsoneControllerTab</class>
+ <widget class="QWidget" name="CsoneControllerTab">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1335</width>
+ <height>937</height>
+ </rect>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="windowTitle">
+ <string>Form</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="2" column="0">
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="4" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="3" column="0">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="topMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>6</number>
+ </property>
+ <item row="0" column="3">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="custom_button_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Button 2</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QPushButton" name="custom_button_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Button 3</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QPushButton" name="custom_button_1">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Button 1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_custom_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_custom_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_custom_1">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="3" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QLabel" name="label_new_title_line2">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLabel" name="label_row_x">
+ <property name="text">
+ <string>x [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLabel" name="label_new_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>New</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_measured_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Measured</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="label_row_y">
+ <property name="text">
+ <string>y [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_5">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QFrame" name="red_frame_position_left">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_measured_title_line2">
+ <property name="text">
+ <string>Position</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="red_frame_position_right">
+ <property name="maximumSize">
+ <size>
+ <width>10</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLabel" name="label_row_z">
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLabel" name="label_row_yaw">
+ <property name="text">
+ <string>yaw [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="3">
+ <widget class="QPushButton" name="set_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set New</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_current_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Current</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="1">
+ <widget class="QLabel" name="label_row_pitch">
+ <property name="text">
+ <string>pitch [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_current_title_line2">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="2">
+ <widget class="QPushButton" name="default_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="QLabel" name="label_row_roll">
+ <property name="text">
+ <string>roll [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="4" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_pitch">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="7" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_12">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_roll">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
index 7bafd6f49857ce5a3ebe291dbe62805069eaf7f8..d92fc08b2c7ba41f048c17b9d4c7bcdee4802863 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
@@ -30,8 +30,8 @@
<property name="spacing">
<number>12</number>
</property>
- <item row="1" column="1">
- <widget class="QPushButton" name="load_yaml_default_button">
+ <item row="1" column="5">
+ <widget class="QPushButton" name="load_yaml_tuning_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -51,12 +51,12 @@
</size>
</property>
<property name="text">
- <string>Default</string>
+ <string>Tuning</string>
</property>
</widget>
</item>
- <item row="1" column="3">
- <widget class="QPushButton" name="load_yaml_picker_button">
+ <item row="1" column="7">
+ <widget class="QPushButton" name="load_yaml_template_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -76,71 +76,43 @@
</size>
</property>
<property name="text">
- <string>Picker</string>
+ <string>Template</string>
</property>
</widget>
</item>
- <item row="0" column="0">
- <widget class="QLabel" name="enable_controller_label">
+ <item row="0" column="2">
+ <widget class="QPushButton" name="enable_student_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>0</width>
+ <width>60</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
- <width>16777215</width>
+ <width>180</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
- <weight>75</weight>
- <bold>true</bold>
+ <weight>50</weight>
+ <bold>false</bold>
</font>
</property>
<property name="text">
- <string>Enable</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QPushButton" name="load_yaml_tuning_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Tuning</string>
+ <string>Student</string>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="load_yaml_label">
+ <item row="0" column="0">
+ <widget class="QLabel" name="enable_controller_label">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -166,7 +138,7 @@
</font>
</property>
<property name="text">
- <string>Load YAML</string>
+ <string>Enable</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
@@ -198,8 +170,8 @@
</property>
</widget>
</item>
- <item row="1" column="6">
- <widget class="QPushButton" name="load_yaml_template_button">
+ <item row="0" column="5">
+ <widget class="QPushButton" name="enable_tuning_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -219,12 +191,12 @@
</size>
</property>
<property name="text">
- <string>Template</string>
+ <string>Tuning</string>
</property>
</widget>
</item>
- <item row="0" column="2">
- <widget class="QPushButton" name="enable_student_button">
+ <item row="0" column="1">
+ <widget class="QPushButton" name="enable_default_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -250,12 +222,12 @@
</font>
</property>
<property name="text">
- <string>Student</string>
+ <string>Default</string>
</property>
</widget>
</item>
<item row="0" column="4">
- <widget class="QPushButton" name="enable_tuning_button">
+ <widget class="QPushButton" name="enable_picker_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -274,13 +246,19 @@
<height>50</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
<property name="text">
- <string>Tuning</string>
+ <string>Picker</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QPushButton" name="enable_default_button">
+ <item row="0" column="7">
+ <widget class="QPushButton" name="enable_template_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -306,43 +284,46 @@
</font>
</property>
<property name="text">
- <string>Default</string>
+ <string>Template</string>
</property>
</widget>
</item>
- <item row="0" column="3">
- <widget class="QPushButton" name="enable_picker_button">
+ <item row="1" column="0">
+ <widget class="QLabel" name="load_yaml_label">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
- <width>60</width>
+ <width>0</width>
<height>50</height>
</size>
</property>
<property name="maximumSize">
<size>
- <width>180</width>
+ <width>16777215</width>
<height>50</height>
</size>
</property>
<property name="font">
<font>
- <weight>50</weight>
- <bold>false</bold>
+ <weight>75</weight>
+ <bold>true</bold>
</font>
</property>
<property name="text">
- <string>Picker</string>
+ <string>Load YAML</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="0" column="6">
- <widget class="QPushButton" name="enable_template_button">
+ <item row="1" column="1">
+ <widget class="QPushButton" name="load_yaml_default_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -361,18 +342,12 @@
<height>50</height>
</size>
</property>
- <property name="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
<property name="text">
- <string>Template</string>
+ <string>Default</string>
</property>
</widget>
</item>
- <item row="0" column="7">
+ <item row="0" column="8">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
@@ -385,7 +360,7 @@
</property>
</spacer>
</item>
- <item row="0" column="5">
+ <item row="0" column="6">
<widget class="QPushButton" name="enable_remote_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
@@ -410,7 +385,7 @@
</property>
</widget>
</item>
- <item row="1" column="5">
+ <item row="1" column="6">
<widget class="QPushButton" name="load_yaml_remote_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
@@ -435,6 +410,81 @@
</property>
</widget>
</item>
+ <item row="1" column="4">
+ <widget class="QPushButton" name="load_yaml_picker_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Picker</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QPushButton" name="enable_csone_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>49</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>CS1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QPushButton" name="load_yaml_csone_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>CS1</string>
+ </property>
+ </widget>
+ </item>
</layout>
</item>
</layout>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
index e6f37426b294f7ef5deda40f757c4c081adcf752..aa37362f82fe3ccd8e170ddccc5d29abb8e9f4e9 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
@@ -190,7 +190,7 @@
<x>0</x>
<y>0</y>
<width>1862</width>
- <height>40</height>
+ <height>26</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -212,6 +212,7 @@
</property>
<addaction name="action_showHideController_default"/>
<addaction name="action_showHideController_student"/>
+ <addaction name="action_showHideController_csone"/>
<addaction name="action_showHideController_picker"/>
<addaction name="action_showHideController_tuning"/>
<addaction name="action_showHideController_remote"/>
@@ -332,6 +333,17 @@
<string>Remote</string>
</property>
</action>
+ <action name="action_showHideController_csone">
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>CS1</string>
+ </property>
+ </action>
</widget>
<layoutdefault spacing="6" margin="11"/>
<customwidgets>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
index 723bdf8da175f7ea13ff3db977ae2cbc79d87b62..3b5dab86e56f6f92d44d759e23a73eb6db9e91d4 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h
@@ -115,6 +115,7 @@
#define TUNING_CONTROLLER 6
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
+#define CSONE_CONTROLLER 9
#define TESTMOTORS_CONTROLLER 21
@@ -128,6 +129,7 @@
#define CMD_USE_TUNING_CONTROLLER 6
#define CMD_USE_PICKER_CONTROLLER 7
#define CMD_USE_TEMPLATE_CONTROLLER 8
+#define CMD_USE_CSONE_CONTROLLER 9
#define CMD_CRAZYFLY_TAKE_OFF 11
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
index 74f0e5b0c1882a679a3f0691764e062e18de398c..41af9a3b5221b6450788833d1639be8a3ac39433 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
@@ -95,6 +95,7 @@ public:
void showHideController_tuning_changed();
void showHideController_remote_changed();
void showHideController_template_changed();
+ void showHideController_csone_changed();
public slots:
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
new file mode 100644
index 0000000000000000000000000000000000000000..b85bfbd139590f6c2f53e457ba65d51819ab9a91
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
@@ -0,0 +1,176 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// The GUI for a Csone Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#ifndef CSONECONTROLLERTAB_H
+#define CSONECONTROLLERTAB_H
+
+#include <QWidget>
+#include <QMutex>
+#include <QVector>
+#include <QLineEdit>
+#include <QTextStream>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+//#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+//#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace dfall_pkg;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+
+
+
+namespace Ui {
+class CsoneControllerTab;
+}
+
+class CsoneControllerTab : public QWidget
+{
+ Q_OBJECT
+
+public:
+ explicit CsoneControllerTab(QWidget *parent = 0);
+ ~CsoneControllerTab();
+
+
+
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void poseDataUnavailableSlot();
+
+
+
+private slots:
+ void on_lineEdit_setpoint_new_x_returnPressed();
+ void on_lineEdit_setpoint_new_y_returnPressed();
+ void on_lineEdit_setpoint_new_z_returnPressed();
+ void on_lineEdit_setpoint_new_yaw_returnPressed();
+
+ void on_set_setpoint_button_clicked();
+
+ void on_default_setpoint_button_clicked();
+
+ void on_custom_button_1_clicked();
+ void on_custom_button_2_clicked();
+ void on_custom_button_3_clicked();
+
+
+
+
+
+private:
+ Ui::CsoneControllerTab *ui;
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+
+
+#ifdef CATKIN_MAKE
+ // PUBLISHER
+ // > For requesting the setpoint to be changed
+ ros::Publisher requestSetpointChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
+
+ // PUBLISHER
+ // > For notifying that a custom button is pressed
+ ros::Publisher customButtonPublisher;
+#endif
+
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+#ifdef CATKIN_MAKE
+ // For receiving message that the setpoint was changed
+ void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+
+ // Publish a message when a custom button is pressed
+ void publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer);
+
+ // Fill the header for a message
+ void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
+ void fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+#endif
+
+ void publishSetpoint(float x, float y, float z, float yaw_degrees);
+
+};
+
+#endif // CSONECONTROLLERTAB_H
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
index c48d711f9b009dd51da036b2dae4c56b41890c9b..c411e2d12fc07b1844615f6ed68c843a239e9aa0 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
@@ -94,7 +94,7 @@ public:
void showHideController_tuning_changed();
void showHideController_remote_changed();
void showHideController_template_changed();
-
+ void showHideController_csone_changed();
void testMotors_triggered();
@@ -111,6 +111,7 @@ private slots:
void on_enable_tuning_button_clicked();
void on_enable_remote_button_clicked();
void on_enable_template_button_clicked();
+ void on_enable_csone_button_clicked();
// LOAD YAML BUTTONS ON-CLICK CALLBACK
void on_load_yaml_default_button_clicked();
@@ -119,6 +120,7 @@ private slots:
void on_load_yaml_tuning_button_clicked();
void on_load_yaml_remote_button_clicked();
void on_load_yaml_template_button_clicked();
+ void on_load_yaml_csone_button_clicked();
private:
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
index ed4c43b6c4a052caa3768cf7b46d610c6953370a..09cab55768363e2a94de5314c39ba75f9642b009 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
@@ -133,6 +133,7 @@ private slots:
void on_action_showHideController_tuning_changed();
void on_action_showHideController_remote_changed();
void on_action_showHideController_template_changed();
+ void on_action_showHideController_csone_changed();
void on_action_testMotors_triggered();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
index ac62ee9e0272cce08d27414640f317be7999aa90..f2ca9793e3e370064f0162867ee9a828dcf7f4e5 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
@@ -87,6 +87,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->template_controller_tab_widget , &TemplateControllerTab::setMeasuredPose
);
+ QObject::connect(
+ this , &ControllerTabs::measuredPoseValueChanged ,
+ ui->csone_controller_tab_widget , &CsoneControllerTab::setMeasuredPose
+ );
+
// CONNECT THE "MEASUREMENTS UNAVAILABLE" SIGNAL TO
@@ -121,6 +126,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->template_controller_tab_widget , &TemplateControllerTab::poseDataUnavailableSlot
);
+ QObject::connect(
+ this , &ControllerTabs::poseDataUnavailableSignal ,
+ ui->csone_controller_tab_widget , &CsoneControllerTab::poseDataUnavailableSlot
+ );
+
// CONNECT TO THE COORDINATOR SIGNAL TO BE ALWAYS UPDATED
// WITH THE LIST OF AGENT IDs TO COORDINATE
@@ -157,6 +167,11 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ui->template_controller_tab_widget , &TemplateControllerTab::setAgentIDsToCoordinate
);
+ QObject::connect(
+ this , &ControllerTabs::agentIDsToCoordinateChanged ,
+ ui->csone_controller_tab_widget , &CsoneControllerTab::setAgentIDsToCoordinate
+ );
+
@@ -272,6 +287,11 @@ void ControllerTabs::showHideController_template_changed()
showHideController_toggle("Template",ui->template_tab);
}
+void ControllerTabs::showHideController_csone_changed()
+{
+ showHideController_toggle("CS1",ui->csone_tab);
+}
+
@@ -457,6 +477,11 @@ void ControllerTabs::setControllerEnabled(int new_controller)
setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->template_tab );
break;
}
+ case CSONE_CONTROLLER:
+ {
+ setTextColourOfTabLabel( m_tab_text_colour_highlight , ui->csone_tab );
+ break;
+ }
default:
{
//ui->controller_enabled_label->setText("Unknown");
@@ -474,13 +499,14 @@ void ControllerTabs::setAllTabLabelsToNormalColouring()
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 );
+ setTextColourOfTabLabel( m_tab_text_colour_normal , ui->csone_tab );
}
void ControllerTabs::setTextColourOfTabLabel(QColor color , QWidget * tab_widget)
{
// Get the current index of the tab
int current_index_of_tab = ui->controller_tabs_widget->indexOf(tab_widget);
- // Onlz apply the colour is the tab is found
+ // Only apply the colour if the tab is found
if (current_index_of_tab >= 0)
{
ui->controller_tabs_widget->tabBar()->setTabTextColor(current_index_of_tab, color);
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..28ab5cda4b77f99c328e010917f0048f3db48974
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
@@ -0,0 +1,696 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// The GUI for a Csone Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+#include "csonecontrollertab.h"
+#include "ui_csonecontrollertab.h"
+
+CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
+ QWidget(parent),
+ ui(new Ui::CsoneControllerTab)
+{
+ ui->setupUi(this);
+
+ // Hide the two red frames that are used to indcated
+ // when pose data is occluded
+ ui->red_frame_position_left->setVisible(false);
+ ui->red_frame_position_right->setVisible(false);
+
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CSONE CONTROLLER TAB GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[CSONE CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
+ requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::SetpointWithHeader>("CsoneControllerService/RequestSetpointChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("CsoneControllerService/SetpointChanged", 1, &CsoneControllerTab::setpointChangedCallback, this);
+ }
+
+ // CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
+ customButtonPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::CustomButtonWithHeader>("CsoneControllerService/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>("CsoneControllerService/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("[CSONE CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+ }
+
+#endif
+
+}
+
+CsoneControllerTab::~CsoneControllerTab()
+{
+ delete ui;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC U U SSSS TTTTT OOO M M
+// C U U S T O O MM MM
+// C U U SSS T O O M M M
+// C U U S T O O M M
+// CCCC UUU SSSS T OOO M M
+//
+// BBBB U U TTTTT TTTTT OOO N N SSSS
+// B B U U T T O O NN N S
+// BBBB U U T T O O N N N SSS
+// B B U U T T O O N NN S
+// BBBB UUU T T OOO N N SSSS
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void CsoneControllerTab::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("[CSONE CONTROLLER TAB GUI] button " << button_index << " clicked.");
+}
+#endif
+
+
+void CsoneControllerTab::on_custom_button_1_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(1,ui->lineEdit_custom_1);
+#endif
+}
+
+void CsoneControllerTab::on_custom_button_2_clicked()
+{
+#ifdef CATKIN_MAKE
+ publish_custom_button_command(2,ui->lineEdit_custom_2);
+#endif
+}
+
+void CsoneControllerTab::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 CsoneControllerTab::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 CsoneControllerTab::poseDataUnavailableSlot()
+{
+ ui->lineEdit_measured_x->setText("xx.xx");
+ ui->lineEdit_measured_y->setText("xx.xx");
+ ui->lineEdit_measured_z->setText("xx.xx");
+
+ ui->lineEdit_measured_roll->setText("xx.xx");
+ ui->lineEdit_measured_pitch->setText("xx.xx");
+ ui->lineEdit_measured_yaw->setText("xx.xx");
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+//
+// CCCC H H A N N GGGG EEEEE DDDD
+// C H H A A NN N G E D D
+// C HHHHH A A N N N G EEE D D
+// C H H AAAAA N NN G G E D D
+// CCCC H H A A N N GGGG EEEEE DDDD
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void CsoneControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
+{
+ // 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 CsoneControllerTab::publishSetpoint(float x, float y, float z, float yaw_degrees)
+{
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ dfall_pkg::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw) values
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw_degrees * DEG2RAD;
+
+ // Publish the setpoint
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[CSONE CONTROLLER GUI] Published request for setpoint change to: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]");
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[CSONE CONTROLLER GUI] would publish request for: [" << x << ", "<< y << ", "<< z << ", "<< yaw_degrees << "]";
+#endif
+}
+
+
+
+void CsoneControllerTab::on_lineEdit_setpoint_new_x_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_setpoint_new_y_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_setpoint_new_z_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void CsoneControllerTab::on_lineEdit_setpoint_new_yaw_returnPressed()
+{
+ ui->set_setpoint_button->animateClick();
+}
+
+void CsoneControllerTab::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 CsoneControllerTab::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("[CSONE 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 CsoneControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+ // // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<dfall_pkg::GetSetpointService>("CsoneControllerService/GetCurrentSetpoint", false);
+ dfall_pkg::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[CSONE CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+
+ // SUBSCRIBERS
+ // > For receiving message that the setpoint was changed
+ setpointChangedSubscriber = agent_base_nodeHandle.subscribe("CsoneControllerService/SetpointChanged", 1, &CsoneControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // 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 CsoneControllerTab::fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[CSONE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void CsoneControllerTab::fillCustomButtonMessageHeader( dfall_pkg::CustomButtonWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[CSONE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool CsoneControllerTab::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the value of the "type" parameter into a local string variable
+ std::string type_string;
+ if(!nodeHandle.getParam("type", type_string))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[CSONE CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[CSONE CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[CSONE CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[CSONE CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[CSONE CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[CSONE CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[CSONE CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
index 6a1e88f17690a7e56dd5701b4a434dee16f2ed98..8fc5ca827656558a05149d85dc0206b25868d239 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
@@ -120,6 +120,12 @@ void EnableControllerLoadYamlBar::showHideController_template_changed()
ui->load_yaml_template_button->setHidden( !(ui->load_yaml_template_button->isHidden()) );
}
+void EnableControllerLoadYamlBar::showHideController_csone_changed()
+{
+ ui->enable_csone_button ->setHidden( !(ui->enable_csone_button->isHidden()) );
+ ui->load_yaml_csone_button->setHidden( !(ui->load_yaml_csone_button->isHidden()) );
+}
+
// TEST MOTORS
@@ -206,6 +212,16 @@ void EnableControllerLoadYamlBar::on_enable_template_button_clicked()
#endif
}
+void EnableControllerLoadYamlBar::on_enable_csone_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ dfall_pkg::IntWithHeader msg;
+ fillIntMessageHeader(msg);
+ msg.data = CMD_USE_CSONE_CONTROLLER;
+ this->commandPublisher.publish(msg);
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Enable CS1 Controller");
+#endif
+}
@@ -310,6 +326,21 @@ void EnableControllerLoadYamlBar::on_load_yaml_template_button_clicked()
#endif
}
+void EnableControllerLoadYamlBar::on_load_yaml_csone_button_clicked()
+{
+#ifdef CATKIN_MAKE
+ // Create a local variable for the message
+ dfall_pkg::StringWithHeader yaml_filename_msg;
+ // Set for whom this applies to
+ fillStringMessageHeader(yaml_filename_msg);
+ // Specify the data
+ yaml_filename_msg.data = "CsoneController";
+ // Send the message
+ m_requestLoadYamlFilenamePublisher.publish(yaml_filename_msg);
+ // Inform the user that the menu item was selected
+ ROS_INFO("[ENABLE CONTROLLER LOAD YAML GUI BAR] Load CS1 Controller YAML was clicked.");
+#endif
+}
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
index d6fc815ad0a3377c2e17a98b699c7b2130da6dc1..c29b832311aee31dfe59804f74483c6db8382c8d 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
@@ -149,6 +149,8 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ui->action_showHideController_remote->trigger();
// > For the template controller
ui->action_showHideController_template->trigger();
+ // > For the cs1 controller
+ ui->action_showHideController_csone->trigger();
}
@@ -261,6 +263,13 @@ void MainWindow::on_action_showHideController_template_changed()
ui->customWidget_controller_tabs->showHideController_template_changed();
}
+void MainWindow::on_action_showHideController_csone_changed()
+{
+ // Notify the UI elements of this change
+ ui->customWidget_enableControllerLoadYamlBar->showHideController_csone_changed();
+ ui->customWidget_controller_tabs->showHideController_csone_changed();
+}
+
void MainWindow::on_action_testMotors_triggered()
{
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
index f1114564b25b3ee94b149b3fa019231bef8c8f2b..2525931a814445652d3286dcf3f47b2fe83fa9b7 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
@@ -131,6 +131,7 @@
#define TUNING_CONTROLLER 6
#define PICKER_CONTROLLER 7
#define TEMPLATE_CONTROLLER 8
+#define CSONE_CONTROLLER 9
#define TESTMOTORS_CONTROLLER 21
@@ -144,6 +145,7 @@
#define CMD_USE_TUNING_CONTROLLER 6
#define CMD_USE_PICKER_CONTROLLER 7
#define CMD_USE_TEMPLATE_CONTROLLER 8
+#define CMD_USE_CSONE_CONTROLLER 9
#define CMD_CRAZYFLY_TAKE_OFF 11
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..ea482a79d8ca0e95cdd746f2d7fcd9794d0413bf
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -0,0 +1,238 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// A CS1 Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
+//some useful libraries
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <ros/package.h>
+
+// Include the standard message types
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+#include <std_msgs/String.h>
+
+// Include the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+//#include "dfall_pkg/StringWithHeader.h"
+#include "dfall_pkg/SetpointWithHeader.h"
+#include "dfall_pkg/CustomButtonWithHeader.h"
+#include "dfall_pkg/ViconData.h"
+#include "dfall_pkg/Setpoint.h"
+#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/Controller.h"
+#include "dfall_pkg/DebugMsg.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/LoadYamlFromFilename.h"
+#include "dfall_pkg/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// Include other classes
+#include "classes/GetParamtersAndNamespaces.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+
+
+
+
+// Namespacing the package
+using namespace dfall_pkg;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// NOTE: many constants are already defined in the
+// "Constant.h" header file
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+
+
+// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
+// > the mass of the crazyflie, in [grams]
+float yaml_cf_mass_in_grams = 25.0;
+
+// > the frequency at which the controller is running
+float yaml_control_frequency = 200.0;
+
+// > the coefficients of the 16-bit command to thrust conversion
+//std::vector<float> yaml_motorPoly(3);
+std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+
+// The min and max for saturating 16 bit thrust commands
+float yaml_command_sixteenbit_min = 1000;
+float yaml_command_sixteenbit_max = 60000;
+
+// > the default setpoint, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool yaml_shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool yaml_shouldDisplayDebugInfo = false;
+
+// The LQR Controller parameters for "LQR_RATE_MODE"
+std::vector<float> yaml_gainMatrixThrust_NineStateVector = { 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixRollRate = { 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00};
+std::vector<float> yaml_gainMatrixPitchRate = { 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00};
+std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30};
+
+
+
+// The weight of the Crazyflie in Newtons, i.e., mg
+float m_cf_weight_in_newtons = 0.0;
+
+// The location error of the Crazyflie at the "previous" time step
+float m_previous_stateErrorInertial[9];
+
+// The setpoint to be tracked, the ordering is (x,y,z,yaw),
+// with units [meters,meters,meters,radians]
+std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
+
+
+
+
+// ROS Publisher for debugging variables
+ros::Publisher m_debugPublisher;
+
+// ROS Publisher for inform the network about
+// changes to the setpoin
+ros::Publisher m_setpointChangedPublisher;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to
+// complile, but adding the function prototypes here means the the functions
+// can be written below in any order. If the function prototypes are not
+// included then the function need to written below in an order that ensure
+// each function is implemented before it is called from another function,
+// hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR
+// INTO AN (x,y) BODY FRAME ERROR
+void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw);
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
+
+// FOR LOADING THE YAML PARAMETERS
+void timerCallback_initial_load_yaml(const ros::TimerEvent&);
+void isReadyCsoneControllerYamlCallback(const IntWithHeader & msg);
+void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index b41dee020f20f6f27ca04ab41d46919cc14b35ab..ade107f9434fed42dd7acf161534bbded3bdbecd 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -208,6 +208,8 @@ ros::ServiceClient m_pickerController;
ros::ServiceClient m_remoteController;
// The Template controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_templateController;
+// The CS1 controller specified in the FlyingAgentClientConfig.yaml
+ros::ServiceClient m_csoneController;
// The Test Mtoros controller specified in the FlyingAgentClientConfig.yaml
ros::ServiceClient m_testMotorsController;
diff --git a/dfall_ws/src/dfall_pkg/launch/agent.launch b/dfall_ws/src/dfall_pkg/launch/agent.launch
index 0815543d28bece2e63ddf5929e93f3a6126fc254..cccd6020e3fc39aedd190ba90a19c3bdce133683 100755
--- a/dfall_ws/src/dfall_pkg/launch/agent.launch
+++ b/dfall_ws/src/dfall_pkg/launch/agent.launch
@@ -120,6 +120,15 @@
>
</node>
+ <!-- CSONE CONTROLLER -->
+ <node
+ pkg = "dfall_pkg"
+ name = "CsoneControllerService"
+ output = "screen"
+ type = "CsoneControllerService"
+ >
+ </node>
+
<!-- TEST MOTORS CONTROLLER -->
<node
pkg = "dfall_pkg"
diff --git a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
index e6bacb8f6af079883950dd1df95c9b4a49056ca3..6edb428394692da236ffb24a4c522657fb19c334 100755
--- a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
@@ -6,6 +6,7 @@ tuningController: "TuningControllerService/TuningController"
pickerController: "PickerControllerService/PickerController"
remoteController: "RemoteControllerService/RemoteController"
templateController: "TemplateControllerService/TemplateController"
+csoneController: "CsoneControllerService/CsoneController"
testMotorsController: "TestMotorsControllerService/TestMotorsController"
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb2aa311a4a3be29a24be173bf149c6d3ce6a5a3
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -0,0 +1,1008 @@
+// Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+// This file is part of D-FaLL-System.
+//
+// D-FaLL-System is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// D-FaLL-System is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with D-FaLL-System. If not, see <http://www.gnu.org/licenses/>.
+//
+//
+// ----------------------------------------------------------------------------------
+// DDDD FFFFF L L SSSS Y Y SSSS TTTTT EEEEE M M
+// D D F aaa L L S Y Y S T E MM MM
+// D D --- FFFF a a L L --- SSS Y SSS T EEE M M M
+// D D F a aa L L S Y S T E M M
+// DDDD F aa a LLLL LLLL SSSS Y SSSS T EEEEE M M
+//
+//
+// DESCRIPTION:
+// A Csone Controller for students build from
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/CsoneControllerService.h"
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// OOO U U TTTTT EEEEE RRRR
+// O O U U T E R R
+// O O U U T EEE RRRR
+// O O U U T E R R
+// OOO UUU T EEEEE R R
+//
+// CCCC OOO N N TTTTT RRRR OOO L L OOO OOO PPPP
+// C O O NN N T R R O O L L O O O O P P
+// C O O N N N T RRRR O O L L O O O O PPPP
+// C O O N NN T R R O O L L O O O O P
+// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
+// ----------------------------------------------------------------------------------
+
+// This function is the callback that is linked to the "CsoneController"
+// service that is advertised in the main function. This must have arguments
+// that match the "input-output" behaviour defined in the "Controller.srv"
+// file (located in the "srv" folder)
+//
+// The arument "request" is a structure provided to this service with the
+// following two properties:
+//
+// >> request.ownCrazyflie
+// This property is itself a structure of type "CrazyflieData", which is
+// defined in the file "CrazyflieData.msg", and has the following properties
+// string crazyflieName
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// The values in these properties are directly the measurement taken by the
+// motion capture system of the Crazyflie that is to be controlled by this
+// service.
+//
+// >> request.otherCrazyflies
+// This property is an array of "CrazyflieData" structures, what allows access
+// to the motion capture measurements of other Crazyflies.
+//
+// The argument "response" is a structure that is expected to be filled in by
+// this service by this function, it has only the following property
+//
+// >> response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is
+// defined in the file "ControlCommand.msg", and has the following properties:
+// float32 roll The command sent to the Crazyflie for the body frame x-axis
+// float32 pitch The command sent to the Crazyflie for the body frame y-axis
+// float32 yaw The command sent to the Crazyflie for the body frame z-axis
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
+// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
+// > The allowed values for "onboardControllerType" are in the "Defines"
+// section in the header file, they are:
+// - CF_COMMAND_TYPE_MOTORS
+// - CF_COMMAND_TYPE_RATE
+// - CF_COMMAND_TYPE_ANGLE
+//
+// > For most common option to use is CF_COMMAND_TYPE_RATE option.
+//
+// > For the CF_COMMAND_TYPE_RATE optoin:
+// 1) the ".roll", ".ptich", and ".yaw" properties of
+// "response.ControlCommand" specify the angular rate in
+// [radians/second] that will be requested from the PID controllers
+// running in the Crazyflie 2.0 firmware.
+// 2) the ".motorCmd1" to ".motorCmd4" properties of
+// "response.ControlCommand" are the baseline motor commands
+// requested from the Crazyflie, with the adjustment for body rates
+// being added on top of this in the firmware (i.e., as per the
+// code of the "distribute_power" found in the firmware).
+// 3) the axis convention for the roll, pitch, and yaw body rates
+// returned in "response.ControlCommand" should use right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the following is an ASCII art of this convention.
+//
+// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
+//
+// > This is a top view,
+// > M1 to M4 stand for Motor 1 to Motor 4,
+// > "CW" indicates that the motor rotates Clockwise,
+// > "CCW" indicates that the motor rotates Counter-Clockwise,
+// > By right-hand axis convention, the positive z-direction points out
+// of the screen,
+// > This being a "top view" means that the direction of positive thrust
+// produced by the propellers is also out of the screen.
+//
+// ____ ____
+// / \ / \
+// (CW) | M4 | x | M1 | (CCW)
+// \____/\ ^ /\____/
+// \ \ | / /
+// \ \ | / /
+// \ \______ | ______/ /
+// \ | /
+// | | |
+// y <-------------o |
+// | |
+// / _______________ \
+// / / \ \
+// / / \ \
+// ____/ / \ \____
+// / \/ \/ \
+// (CCW) | M3 | | M2 | (CW)
+// \____/ \____/
+//
+//
+//
+//
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+
+ // This is the "start" of the outer loop controller, add all your control
+ // computation here, or you may find it convienient to create functions
+ // to keep you code cleaner
+
+
+ // Define a local array to fill in with the state error
+ float stateErrorInertial[9];
+
+ // Fill in the (x,y,z) position error
+ stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0];
+ stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
+ stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
+
+ // Compute an estimate of the velocity
+ // > As simply the derivative between the current and previous position
+ stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+
+ // Fill in the roll and pitch angle measurements directly
+ stateErrorInertial[6] = request.ownCrazyflie.roll;
+ stateErrorInertial[7] = request.ownCrazyflie.pitch;
+
+ // Fill in the yaw angle error
+ // > This error should be "unwrapped" to be in the range
+ // ( -pi , pi )
+ // > First, get the yaw error into a local variable
+ float yawError = request.ownCrazyflie.yaw - m_setpoint[3];
+ // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
+ while(yawError > PI) {yawError -= 2 * PI;}
+ while(yawError < -PI) {yawError += 2 * PI;}
+ // > Third, put the "yawError" into the "stateError" variable
+ stateErrorInertial[8] = yawError;
+
+
+ // CONVERSION INTO BODY FRAME
+ // Conver the state erorr from the Inertial frame into the Body frame
+ // > Note: the function "convertIntoBodyFrame" is implemented in this file
+ // and by default does not perform any conversion. The equations to convert
+ // the state error into the body frame should be implemented in that function
+ // for successful completion of the classroom exercise
+ float stateErrorBody[9];
+ convertIntoBodyFrame(stateErrorInertial, stateErrorBody, request.ownCrazyflie.yaw);
+
+
+ // SAVE THE STATE ERROR TO BE USED NEXT TIME THIS FUNCTION IS CALLED
+ // > as we have already used previous error we can now update it update it
+ for(int i = 0; i < 9; ++i)
+ {
+ m_previous_stateErrorInertial[i] = stateErrorInertial[i];
+ }
+
+
+
+ // PERFORM THE "u=-Kx" CONTROLLER COMPUTATIONS
+
+ // Instantiate local variables for the responses
+ float thrustAdjustment = 0;
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+
+ // Perform the "-Kx" LQR computation for the yaw rate to respoond with
+ for(int i = 0; i < 9; ++i)
+ {
+ // For the z-controller
+ thrustAdjustment -= yaml_gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ // For the x-controller
+ pitchRate_forResponse -= yaml_gainMatrixPitchRate[i] * stateErrorBody[i];
+ // For the y-controller
+ rollRate_forResponse -= yaml_gainMatrixRollRate[i] * stateErrorBody[i];
+ // For the yaw-controller
+ yawRate_forResponse -= yaml_gainMatrixYawRate[i] * stateErrorBody[i];
+ }
+
+ // Put the computed body rate commands into the "response" variable
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+
+ // Put the thrust commands into the "response" variable.
+ // The thrust adjustment computed by the controller need to be added to the
+ // the feed-forward thrust that "counter-acts" gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ // > NOTE: the "m_cf_weight_in_newtons" value is the total thrust required
+ // as feed-forward. Assuming the the Crazyflie is symmetric, this
+ // value is divided by four.
+ float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
+ float thrust_request_per_motor = thrustAdjustment / 4.0 + feed_forward_thrust_per_motor;
+ // > NOTE: the function "computeMotorPolyBackward" converts the input argument
+ // from Newtons to the 16-bit command expected by the Crazyflie.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrust_request_per_motor);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrust_request_per_motor);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_request_per_motor);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_request_per_motor);
+
+
+ // PREPARE AND RETURN THE VARIABLE "response"
+
+ // Choose the controller type use on-board the Crazyflie,
+ // it can be either be Motor, Rate, or Angle based
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+
+
+ if (yaml_shouldPublishDebugMessage)
+ {
+ // ***********************************************************
+ // DDDD EEEEE BBBB U U GGGG M M SSSS GGGG
+ // D D E B B U U G MM MM S G
+ // D D EEE BBBB U U G M M M SSS G
+ // D D E B B U U G G M M S G G
+ // DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
+
+ // DEBUGGING CODE:
+ // As part of the D-FaLL-System we have defined a message type names"DebugMsg".
+ // By fill this message with data and publishing it you can display the data in
+ // real time using rpt plots. Instructions for using rqt plots can be found on
+ // the wiki of the D-FaLL-System repository
+
+ // Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
+ // (located in the "msg" folder) to see the full structure of this message.
+ DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ debugMsg.vicon_x = request.ownCrazyflie.x;
+ debugMsg.vicon_y = request.ownCrazyflie.y;
+ debugMsg.vicon_z = request.ownCrazyflie.z;
+ debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // Fill in the debugging message with any other data you would like to display
+ // in real time. For example, it might be useful to display the thrust
+ // adjustment computed by the z-altitude controller.
+ // The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
+ // type "float64" that you can fill in with data you would like to plot in
+ // real-time.
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ // Publish the "debugMsg"
+ m_debugPublisher.publish(debugMsg);
+ }
+
+
+ if (yaml_shouldDisplayDebugInfo)
+ {
+ // ***********************************************************
+ // DDDD EEEEE BBBB U U GGGG M M SSSS GGGG
+ // D D E B B U U G MM MM S G
+ // D D EEE BBBB U U G M M M SSS G
+ // D D E B B U U G G M M S G G
+ // DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
+ ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
+ ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
+ ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
+ ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
+ ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
+ ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
+
+ // An example of "printing out" the control actions computed.
+ ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
+ ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
+ ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
+ ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+
+ // An example of "printing out" the per motor 16-bit command computed.
+ ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
+ ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
+
+ // An example of "printing out" the "thrust-to-command" conversion parameters.
+ // ROS_INFO_STREAM("motorPoly 0:" << yaml_motorPoly[0]);
+ // ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
+ // ROS_INFO_STREAM("motorPoly 2:" << yaml_motorPoly[2]);
+ }
+
+ // Return "true" to indicate that the control computation was
+ // performed successfully
+ return true;
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// RRRR OOO TTTTT A TTTTT EEEEE III N N TTTTT OOO
+// R R O O T A A T E I NN N T O O
+// RRRR O O T A A T EEE I N N N T O O
+// R R O O T AAAAA T E I N NN T O O
+// R R OOO T A A T EEEEE III N N T OOO
+//
+// BBBB OOO DDDD Y Y FFFFF RRRR A M M EEEEE
+// B B O O D D Y Y F R R A A MM MM E
+// BBBB O O D D Y FFF RRRR A A M M M EEE
+// B B O O D D Y F R R AAAAA M M E
+// BBBB OOO DDDD Y F R R A A M M EEEEE
+// ----------------------------------------------------------------------------------
+
+// The arguments for this function are as follows:
+// stateInertial
+// This is an array of length 9 with the estimates the error of of the following values
+// relative to the sepcifed setpoint:
+// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
+// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
+// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
+//
+// stateBody
+// This is an empty array of length 9, this function should fill in all elements of this
+// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
+// position and (x,y) velocities are rotated into the body frame.
+//
+// yaw_measured
+// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
+// the Vicon motion capture system
+//
+void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
+{
+ float sinYaw = sin(yaw_measured);
+ float cosYaw = cos(yaw_measured);
+
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0] * cosYaw + stateInertial[1] * sinYaw;
+ stateBody[1] = stateInertial[1] * cosYaw - stateInertial[0] * sinYaw;
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3] * cosYaw + stateInertial[4] * sinYaw;
+ stateBody[4] = stateInertial[4] * cosYaw - stateInertial[3] * sinYaw;
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+}
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ----------------------------------------------------------------------------------
+
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-but command that would produce the requested
+ // "thrust" based on the quadratic mapping that is described
+ // by the coefficients in the "yaml_motorPoly" variable.
+ float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd_16bit < yaml_command_sixteenbit_min)
+ {
+ cmd_16bit = 0;
+ }
+ else if (cmd_16bit > yaml_command_sixteenbit_max)
+ {
+ cmd_16bit = yaml_command_sixteenbit_max;
+ }
+ // Return the result
+ return cmd_16bit;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// NN N E W W S E T P P O O I NN N T
+// N N N EEE W W SSS EEE T PPPP O O I N N N T
+// N NN E W W W S E T P O O I N NN T
+// N N EEEEE W W SSSS EEEEE T P OOO III N N T
+//
+// CCCC A L L BBBB A CCCC K K
+// C A A L L B B A A C K K
+// C A A L L BBBB A A C KKK
+// C AAAAA L L B B AAAAA C K K
+// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+
+// REQUEST SETPOINT CHANGE CALLBACK
+void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Check if the request if for the default setpoint
+ if (newSetpoint.buttonID == REQUEST_DEFAULT_SETPOINT_BUTTON_ID)
+ {
+ setNewSetpoint(
+ yaml_default_setpoint[0],
+ yaml_default_setpoint[1],
+ yaml_default_setpoint[2],
+ yaml_default_setpoint[3]
+ );
+ }
+ else
+ {
+ // Call the function for actually setting the setpoint
+ setNewSetpoint(
+ newSetpoint.x,
+ newSetpoint.y,
+ newSetpoint.z,
+ newSetpoint.yaw
+ );
+ }
+ }
+}
+
+
+// CHANGE SETPOINT FUNCTION
+void setNewSetpoint(float x, float y, float z, float yaw)
+{
+ // Put the new setpoint into the class variable
+ m_setpoint[0] = x;
+ m_setpoint[1] = y;
+ m_setpoint[2] = z;
+ m_setpoint[3] = yaw;
+
+ // Publish the change so that the network is updated
+ // (mainly the "flying agent GUI" is interested in
+ // displaying this change to the user)
+
+ // Instantiate a local variable of type "SetpointWithHeader"
+ SetpointWithHeader msg;
+ // Fill in the setpoint
+ msg.x = x;
+ msg.y = y;
+ msg.z = z;
+ msg.yaw = yaw;
+ // Publish the message
+ m_setpointChangedPublisher.publish(msg);
+}
+
+
+// GET CURRENT SETPOINT SERVICE CALLBACK
+bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response)
+{
+ // Directly put the current setpoint into the response
+ response.setpointWithHeader.x = m_setpoint[0];
+ response.setpointWithHeader.y = m_setpoint[1];
+ response.setpointWithHeader.z = m_setpoint[2];
+ response.setpointWithHeader.yaw = m_setpoint[3];
+ // Return
+ return true;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC U U SSSS TTTTT OOO M M
+// C U U S T O O MM MM
+// C U U SSS T O O M M M
+// C U U S T O O M M
+// CCCC UUU SSSS T OOO M M
+//
+// CCCC OOO M M M M A N N DDDD
+// C O O MM MM MM MM A A NN N D D
+// C O O M M M M M M A A N N N D D
+// C O O M M M M AAAAA N NN D D
+// CCCC OOO M M M M A A N N DDDD
+// ----------------------------------------------------------------------------------
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , commandReceived.shouldCheckForAgentID , commandReceived.agentIDs );
+
+ if (isRevelant)
+ {
+ // Extract the data from the message
+ int custom_button_index = commandReceived.button_index;
+ float float_data = commandReceived.float_data;
+ //std::string string_data = commandReceived.string_data;
+
+ // Switch between the button pressed
+ switch(custom_button_index)
+ {
+
+ // > FOR CUSTOM BUTTON 1
+ case 1:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
+ // Code here to respond to custom button 1
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 2
+ case 2:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
+ // Code here to respond to custom button 2
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 3
+ case 3:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Button 3 received in controller, with message.float_data = " << float_data );
+ // Code here to respond to custom button 3
+
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("[CSONE CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.float_data = " << float_data );
+ break;
+ }
+ }
+ }
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+
+// TIMER CALLBACK FOR SENDING THE LOAD YAML REQUEST
+void timerCallback_initial_load_yaml(const ros::TimerEvent&)
+{
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ // Create the service client as a local variable
+ ros::ServiceClient requestLoadYamlFilenameServiceClient = nodeHandle_to_own_agent_parameter_service.serviceClient<LoadYamlFromFilename>("requestLoadYamlFilename", false);
+ // Create the service call as a local variable
+ LoadYamlFromFilename loadYamlFromFilenameCall;
+ // Specify the Yaml filename as a string
+ loadYamlFromFilenameCall.request.stringWithHeader.data = "CsoneController";
+ // Set for whom this applies to
+ loadYamlFromFilenameCall.request.stringWithHeader.shouldCheckForAgentID = false;
+ // Wait until the serivce exists
+ requestLoadYamlFilenameServiceClient.waitForExistence(ros::Duration(-1));
+ // Make the service call
+ if(requestLoadYamlFilenameServiceClient.call(loadYamlFromFilenameCall))
+ {
+ // Nothing to do in this case.
+ // The "isReadyDefaultControllerYamlCallback" function
+ // will be called once the YAML file is loaded
+ }
+ else
+ {
+ // Inform the user
+ ROS_ERROR("[CSONE CONTROLLER] The request load yaml file service call failed.");
+ }
+}
+
+
+// CALLBACK NOTIFYING THAT THE YAML PARAMETERS ARE READY TO BE LOADED
+void isReadyCsoneControllerYamlCallback(const IntWithHeader & msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Initialise a local variable for the namespace
+ std::string namespace_to_use;
+ // Load from the respective parameter service
+ switch(parameter_service_to_load_from)
+ {
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ ROS_INFO("[CSONE CONTROLLER] Now fetching the CsoneController YAML parameter values from this agent.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case LOAD_YAML_FROM_COORDINATOR:
+ {
+ ROS_INFO("[CSONE CONTROLLER] Now fetching the CsoneController YAML parameter values from this agent's coordinator.");
+ namespace_to_use = m_namespace_to_coordinator_parameter_service;
+ break;
+ }
+
+ default:
+ {
+ ROS_ERROR("[CSONE CONTROLLER] Paramter service to load from was NOT recognised.");
+ namespace_to_use = m_namespace_to_own_agent_parameter_service;
+ break;
+ }
+ }
+ // Create a node handle to the selected parameter service
+ ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+ // Call the function that fetches the parameters
+ fetchCsoneControllerYamlParameters(nodeHandle_to_use);
+ }
+}
+
+
+// LOADING OF THE YAML PARAMTERS
+void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // CsoneController.yaml
+
+ // Add the "CsoneController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "CsoneController");
+
+
+
+ // GET THE PARAMETERS:
+
+ // > The mass of the crazyflie
+ yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
+
+ // > The frequency at which the "computeControlOutput" is being called,
+ // as determined by the frequency at which the motion capture system
+ // provides position and attitude data
+ yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor
+ // command to thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+
+ // > The min and max for saturating 16 bit thrust commands
+ yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
+ yaml_command_sixteenbit_max = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_max");
+
+ // The default setpoint, the ordering is (x,y,z,yaw),
+ // with unit [meters,meters,meters,radians]
+ getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+
+ // Boolean indiciating whether the "Debug Message" of this agent
+ // should be published or not
+ yaml_shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paramaters, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should
+ // be displayed or not
+ yaml_shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
+
+ // The LQR Controller parameters
+ // The LQR Controller parameters for "LQR_MODE_RATE"
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
+
+ // > DEBUGGING: Print out one of the parameters that was loaded to
+ // debug if the fetching of parameters worked correctly
+ ROS_INFO_STREAM("[CSONE CONTROLLER] DEBUGGING: the fetched CsoneController/mass = " << yaml_cf_mass_in_grams);
+
+
+
+ // PROCESS THE PARAMTERS
+
+ // > Compute the feed-forward force that we need to counteract
+ // gravity (i.e., mg) in units of [Newtons]
+ m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
+
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[CSONE CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+
+
+// This function does NOT need to be edited
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "CsoneControllerService");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle"
+ // as the current node, the "~" indcates that "self" is the
+ // node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this "CsoneControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[CSONE CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+ // AGENT ID AND COORDINATOR ID
+
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the
+ // "FlyingAgentClient" node.
+ // > Thus, to get access to this "agentID" paremeter, we first
+ // need to get a handle to the "FlyingAgentClient" node within which this
+ // controller service is nested.
+
+
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[CSONE CONTROLLER] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[CSONE CONTROLLER] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
+
+
+
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+ // NOTES:
+ // > The parameters that are specified thorugh the *.yaml files
+ // are managed by a separate node called the "Parameter Service"
+ // > A separate node is used for reasons of speed and generality
+ // > To allow for a distirbuted architecture, there are two
+ // "ParamterService" nodes that are relevant:
+ // 1) the one that is nested under the "m_agentID" namespace
+ // 2) the one that is nested under the "m_coordID" namespace
+ // > The following lines of code create the namespace (as strings)
+ // to there two relevant "ParameterService" nodes.
+ // > The node handles are also created because they are needed
+ // for the ROS Subscriptions that follow.
+
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for this agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Set the class variable "m_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+ // Inform the user of what namespaces are being used
+ ROS_INFO_STREAM("[CSONE CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[CSONE CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+
+
+
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameter service publishes messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber safeContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "CsoneController", 1, isReadyCsoneControllerYamlCallback);
+ ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("CsoneController", 1, isReadyCsoneControllerYamlCallback);
+
+
+
+ // GIVE YAML VARIABLES AN INITIAL VALUE
+ // This can be done either here or as part of declaring the
+ // variables in the header file
+
+
+
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+ // The yaml files for the controllers are not added to
+ // "Parameter Service" as part of launching.
+ // The process for loading the yaml parameters is to send a
+ // service call containing the filename of the *.yaml file,
+ // and then a message will be received on the above subscribers
+ // when the paramters are ready.
+ // > NOTE IMPORTANTLY that by using a service client
+ // we stall the availability of this node until the
+ // paramter service is ready
+ // > NOTE FURTHER that calling on the service directly from here
+ // often means the yaml file is not actually loaded. So we
+ // instead use a timer to delay the loading
+
+ // Create a single-shot timer
+ ros::Timer timer_initial_load_yaml = nodeHandle.createTimer(ros::Duration(2.0), timerCallback_initial_load_yaml, true);
+ timer_initial_load_yaml.start();
+
+
+
+
+ // PUBLISHERS AND SUBSCRIBERS
+
+ // Instantiate the class variable "m_debugPublisher" to be a
+ // "ros::Publisher". This variable advertises under the name
+ // "DebugTopic" and is a message with the structure defined
+ // in the file "DebugMsg.msg" (located in the "msg" folder).
+ m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "requestSetpointChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestSetpointChange" topic and calls the class function
+ // "requestSetpointChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // the callback function. This subscriber will mainly receive
+ // messages from the "flying agent GUI" when the setpoint is changed
+ // by the user.
+ ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ std::string namespace_to_coordinator;
+ constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("CsoneControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+
+ // Instantiate the class variable "m_setpointChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "SetpointChanged" and is a message with the structure defined
+ // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // field that displays the current setpoint for this controller.
+ m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
+
+ // Instantiate the local variable "getCurrentSetpointService" to be
+ // a "ros::ServiceServer" type variable that advertises the service
+ // called "GetCurrentSetpoint". This service has the input-output
+ // behaviour defined in the "GetSetpointService.srv" file (located
+ // in the "srv" folder). This service, when called, is provided with
+ // an integer (that is essentially ignored), and is expected to respond
+ // with the current setpoint of the controller. When a request is made
+ // of this service the "getCurrentSetpointCallback" function is called.
+ ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);
+
+
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "CsoneController". This service has
+ // the input-output behaviour defined in the "Controller.srv" file (located in the
+ // "srv" folder). This service, when called, is provided with the most recent
+ // measurement of the Crazyflie and is expected to respond with the control action
+ // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
+ // this is where the "outer loop" controller function starts. When a request is made
+ // of this service the "calculateControlOutput" function is called.
+ ros::ServiceServer service = nodeHandle.advertiseService("CsoneController", calculateControlOutput);
+
+ // Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "GUIButton" topic and calls the class
+ // function "customCommandReceivedCallback" each time a messaged is received on this topic
+ // and the message received is passed as an input argument to the callback function.
+ ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("CustomButtonPressed", 1, customCommandReceivedCallback);
+
+ // Same again but instead for changes requested by the coordinator.
+ // For this we need to first create a node handle to the coordinator:
+ //std::string namespace_to_coordinator;
+ //constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+ //ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // And now we can instantiate the subscriber:
+ ros::Subscriber customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("CsoneControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
+
+
+
+ // Print out some information to the user.
+ ROS_INFO("[CSONE CONTROLLER] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
+}
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 1f36a2cb7fa3f262c25e285daea14f8ded4fbf05..0a506bd29d77df0a2a9526d445b6861a892ade05 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -844,6 +844,9 @@ void setInstantController(int controller)
case TEMPLATE_CONTROLLER:
m_instant_controller_service_client = &m_templateController;
break;
+ case CSONE_CONTROLLER:
+ m_instant_controller_service_client = &m_csoneController;
+ break;
case TESTMOTORS_CONTROLLER:
m_instant_controller_service_client = &m_defaultController;
break;
@@ -934,6 +937,8 @@ int getControllerNominallySelected()
// > The options are: {take-off,land,motor-off,controller}
void flyingStateRequestCallback(const IntWithHeader & msg) {
+ ROS_INFO("csone buggggggs");
+
// Check whether the message is relevant
bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
@@ -984,6 +989,11 @@ void flyingStateRequestCallback(const IntWithHeader & msg) {
setControllerNominallySelected(TEMPLATE_CONTROLLER);
break;
+ case CMD_USE_CSONE_CONTROLLER:
+ ROS_INFO("[FLYING AGENT CLIENT] USE_CSONE_CONTROLLER Command received");
+ setControllerNominallySelected(CSONE_CONTROLLER);
+ break;
+
case CMD_CRAZYFLY_TAKE_OFF:
ROS_INFO("[FLYING AGENT CLIENT] TAKE_OFF Command received");
@@ -1277,6 +1287,7 @@ void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
createControllerServiceClientFromParameterName( "pickerController" , m_pickerController );
createControllerServiceClientFromParameterName( "remoteController" , m_remoteController );
createControllerServiceClientFromParameterName( "templateController" , m_templateController );
+ createControllerServiceClientFromParameterName( "csoneController" , m_csoneController );
createControllerServiceClientFromParameterName( "testMotorsController" , m_testMotorsController );