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 d92fc08b2c7ba41f048c17b9d4c7bcdee4802863..f52ce5f922393979375c5b028f8b97f41e9d0857 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
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1572</width>
+ <width>1784</width>
<height>403</height>
</rect>
</property>
@@ -30,87 +30,6 @@
<property name="spacing">
<number>12</number>
</property>
- <item row="1" column="5">
- <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>
- </property>
- </widget>
- </item>
- <item row="1" column="7">
- <widget class="QPushButton" name="load_yaml_template_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>Template</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QPushButton" name="enable_student_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="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
- <property name="text">
- <string>Student</string>
- </property>
- </widget>
- </item>
<item row="0" column="0">
<widget class="QLabel" name="enable_controller_label">
<property name="sizePolicy">
@@ -145,149 +64,6 @@
</property>
</widget>
</item>
- <item row="1" column="2">
- <widget class="QPushButton" name="load_yaml_student_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Student</string>
- </property>
- </widget>
- </item>
- <item row="0" column="5">
- <widget class="QPushButton" name="enable_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>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="enable_default_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="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
- <property name="text">
- <string>Default</string>
- </property>
- </widget>
- </item>
- <item row="0" column="4">
- <widget class="QPushButton" name="enable_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="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
- <property name="text">
- <string>Picker</string>
- </property>
- </widget>
- </item>
- <item row="0" column="7">
- <widget class="QPushButton" name="enable_template_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="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
- <property name="text">
- <string>Template</string>
- </property>
- </widget>
- </item>
<item row="1" column="0">
<widget class="QLabel" name="load_yaml_label">
<property name="sizePolicy">
@@ -322,167 +98,440 @@
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QPushButton" name="load_yaml_default_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>Default</string>
- </property>
- </widget>
- </item>
- <item row="0" column="8">
- <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="6">
- <widget class="QPushButton" name="enable_remote_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>Remote</string>
- </property>
- </widget>
- </item>
- <item row="1" column="6">
- <widget class="QPushButton" name="load_yaml_remote_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>Remote</string>
- </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>
+ <item row="1" column="2">
+ <widget class="QWidget" name="widget_yamlButtons" native="true">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QPushButton" name="load_yaml_default_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>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="load_yaml_student_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Student</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <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>
+ <item>
+ <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>
+ <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>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="load_yaml_template_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>Template</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="load_yaml_remote_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>Remote</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</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>
+ <item row="0" column="2">
+ <widget class="QWidget" name="widget_enableButtons" native="true">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QPushButton" name="enable_default_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="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_student_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="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Student</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <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>
+ <widget class="QPushButton" name="enable_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="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Picker</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_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>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_template_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="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Template</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_remote_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>Remote</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</widget>
</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 aa37362f82fe3ccd8e170ddccc5d29abb8e9f4e9..401d1042c7eac0c7f10b21c1d6f66b94ba5d6840 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,14 +190,14 @@
<x>0</x>
<y>0</y>
<width>1862</width>
- <height>26</height>
+ <height>47</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
<property name="title">
<string>File</string>
</property>
- <addaction name="actionShowHide_Coordinator"/>
+ <addaction name="action_showHide_Coordinator"/>
</widget>
<widget class="QMenu" name="menuLoad_YAML">
<property name="title">
@@ -205,6 +205,8 @@
</property>
<addaction name="action_LoadYAML_BatteryMonitor"/>
<addaction name="action_LoadYAML_FlyingAgentClientConfig"/>
+ <addaction name="separator"/>
+ <addaction name="action_showHide_loadYamlBar"/>
</widget>
<widget class="QMenu" name="menuControllers">
<property name="title">
@@ -233,7 +235,7 @@
</attribute>
</widget>
<widget class="QStatusBar" name="statusBar"/>
- <action name="actionShowHide_Coordinator">
+ <action name="action_showHide_Coordinator">
<property name="text">
<string>Hide Coordinator</string>
</property>
@@ -344,6 +346,11 @@
<string>CS1</string>
</property>
</action>
+ <action name="action_showHide_loadYamlBar">
+ <property name="text">
+ <string>Hide Yaml Buttons</string>
+ </property>
+ </action>
</widget>
<layoutdefault spacing="6" margin="11"/>
<customwidgets>
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 c411e2d12fc07b1844615f6ed68c843a239e9aa0..888f6cad5dd905d34adfdc0eb07304ae6604ff6c 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
@@ -95,9 +95,15 @@ public:
void showHideController_remote_changed();
void showHideController_template_changed();
void showHideController_csone_changed();
+
+ // PUBLIC METHOD FOR TOGGLING THE VISIBILITY OF THE YAML BAR
+ bool showHide_loadYamlBar_triggered();
+
+ // PUBLIC METHODS FOR ENABLING THE TEST MOTORS CONTROLLER
void testMotors_triggered();
+
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
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 09cab55768363e2a94de5314c39ba75f9642b009..63a0e37630ce6f13d0a847caa0a9df5c8ffbf75d 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
@@ -122,7 +122,8 @@ private:
private slots:
// PRIVATE METHODS FOR MENU ITEM CALLBACKS
- void on_actionShowHide_Coordinator_triggered();
+ void on_action_showHide_Coordinator_triggered();
+ void on_action_showHide_loadYamlBar_triggered();
void on_action_LoadYAML_BatteryMonitor_triggered();
void on_action_LoadYAML_FlyingAgentClientConfig_triggered();
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 8fc5ca827656558a05149d85dc0206b25868d239..31c95bdbcf0faae262b40e0220adca4cf7eab362 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
@@ -126,6 +126,24 @@ void EnableControllerLoadYamlBar::showHideController_csone_changed()
ui->load_yaml_csone_button->setHidden( !(ui->load_yaml_csone_button->isHidden()) );
}
+bool EnableControllerLoadYamlBar::showHide_loadYamlBar_triggered()
+{
+ bool bar_isHidden;
+ if (ui->load_yaml_label->isHidden())
+ {
+ ui->load_yaml_label->show();
+ ui->widget_yamlButtons->show();
+ bar_isHidden = false;
+ }
+ else
+ {
+ ui->load_yaml_label->hide();
+ ui->widget_yamlButtons->hide();
+ bar_isHidden = true;
+ }
+ return bar_isHidden;
+}
+
// TEST MOTORS
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 c29b832311aee31dfe59804f74483c6db8382c8d..e61244c2865eda33a47440de3c75c0be75fb3a9e 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
@@ -91,7 +91,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
// Hide the coordinator part of the GUI
ui->customWidget_coordinator->hide();
// And make the menu item unavailable
- ui->actionShowHide_Coordinator->setEnabled(false);
+ ui->action_showHide_Coordinator->setEnabled(false);
}
#endif
@@ -160,7 +160,7 @@ MainWindow::~MainWindow()
}
-void MainWindow::on_actionShowHide_Coordinator_triggered()
+void MainWindow::on_action_showHide_Coordinator_triggered()
{
//ui->customWidget_enableControllerLoadYamlBar->setEnabled(false);
if ( ui->customWidget_coordinator->isHidden() )
@@ -168,14 +168,32 @@ void MainWindow::on_actionShowHide_Coordinator_triggered()
ui->customWidget_coordinator->show();
ui->coordinator_to_main_panel_vertical_line->show();
QString qstr = "Hide Coordinator";
- ui->actionShowHide_Coordinator->setText(qstr);
+ ui->action_showHide_Coordinator->setText(qstr);
}
else
{
ui->customWidget_coordinator->hide();
ui->coordinator_to_main_panel_vertical_line->hide();
QString qstr = "Show Coordinator";
- ui->actionShowHide_Coordinator->setText(qstr);
+ ui->action_showHide_Coordinator->setText(qstr);
+ }
+}
+
+
+void MainWindow::on_action_showHide_loadYamlBar_triggered()
+{
+ // Trigger the function to show/hide the bar
+ bool bar_isHidden = ui->customWidget_enableControllerLoadYamlBar->showHide_loadYamlBar_triggered();
+ // Update the text of the menu item
+ if ( bar_isHidden )
+ {
+ QString qstr = "Show Yaml Buttons";
+ ui->action_showHide_loadYamlBar->setText(qstr);
+ }
+ else
+ {
+ QString qstr = "Hide Yaml Buttons";
+ ui->action_showHide_loadYamlBar->setText(qstr);
}
}
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
index 5d17177cbf0fff1ceafa4143166a2ed65946b9c7..9b4cfae589e46b806c973306b73714ad3d85f4fe 100755
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
@@ -206,7 +206,7 @@ private:
#ifdef CATKIN_MAKE
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
- void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+ void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
diff --git a/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
index 5c9f4339e01de648619cb7a4e3b71f2ef88fc032..fb35125283a24eb5aef812eb98eee073c3031b74 100644
--- a/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
+++ b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
@@ -107,15 +107,19 @@ using namespace dfall_pkg;
// GET YAML PARAMETER FUNCTIONS
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterStringVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<std::string>& val, int length);
+int getParameterStringVectorUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<std::string>& val);
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterFloatVectorUnnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val);
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
-std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
+
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h b/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h
index d298e9e2b2456acf03f5aac57ed73ba69d341ee5..bed209750ff56561b977b3f6568dd38f94210bea 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h
@@ -43,6 +43,7 @@
#include <stdlib.h>
#include <string>
+#include <map>
#include <ros/ros.h>
#include <ros/package.h>
@@ -65,6 +66,7 @@
#include "nodes/Constants.h"
+
// ----------------------------------------------------------------------------------
// DDDD EEEEE FFFFF III N N EEEEE SSSS
// D D E F I NN N E S
@@ -106,8 +108,15 @@ std::string m_base_namespace;
// Publisher for passing a service request onto the
// loadinging function
-ros::Publisher requestLoadYamlFilenamePublisher;
+ros::Publisher m_requestLoadYamlFilenamePublisher;
+
+// Vector of strings loaded from YAML file with the
+// file names of yaml files to load
+std::vector<std::string> yaml_filenames_provided;
+// Map of Publishers for informing the network that this
+// Parameter service node loaded a requested YAML file
+std::map<std::string, ros::Publisher> m_yamlParametersReadyForFetchPublisherMap;
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h
index 3a540f297544dfa279d053e34d8265c3e606ba61..f7c666e3ecc0d2892df18664cd2ff6804f9ffa38 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h
@@ -199,7 +199,7 @@ void processFetchedParameters();
// > For the GETPARAM()
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
diff --git a/dfall_ws/src/dfall_pkg/launch/agent.launch b/dfall_ws/src/dfall_pkg/launch/agent.launch
index fe0977ca4097fc1e664f0fa158e10418db05fe82..c321290521b1c3ed86a0709de4c260bd297a6fb8 100755
--- a/dfall_ws/src/dfall_pkg/launch/agent.launch
+++ b/dfall_ws/src/dfall_pkg/launch/agent.launch
@@ -147,6 +147,11 @@
>
<param name="type" type="str" value="agent" />
<param name="agentID" value="$(arg agentID)" />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/YamlFileNames.yaml"
+ ns = "YamlFileNames"
+ />
<rosparam
command = "load"
file = "$(find dfall_pkg)/param/FlyingAgentClientConfig.yaml"
diff --git a/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
index 6e9de18ca30ef0ded472a05f1915839aaef1ea73..94ce721da5000f068ef9ec3f5581dc7565d3f456 100644
--- a/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
+++ b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
@@ -1,11 +1,11 @@
-dictionary : {
- 'BatteryMonitor' : 'BatteryMonitor',
- 'FlyingAgentClientConfig' : 'FlyingAgentClientConfig',
- 'CsoneControllerService' : 'CsoneControllerService',
- 'DefaultControllerService' : 'DefaultControllerService',
- 'PickerControllerService' : 'PickerControllerService',
- 'RemoteControllerService' : 'RemoteControllerService',
- 'StudentControllerService' : 'StudentControllerService',
- 'TemplateControllerService' : 'TemplateControllerService',
- 'TuningControllerService' : 'TuningControllerService'
-}
+filenames : [
+ 'BatteryMonitor',
+ 'FlyingAgentClientConfig',
+ 'CsoneControllerService',
+ 'DefaultControllerService',
+ 'PickerControllerService',
+ 'RemoteControllerService',
+ 'StudentControllerService',
+ 'TemplateControllerService',
+ 'TuningControllerService'
+]
diff --git a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
index c299932b98a82a2610be415b7c806847f9ab2091..63e50c816c1214ba7efb80389d09ac8c6a11845d 100644
--- a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
@@ -78,79 +78,105 @@ std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
{
std::string val;
if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
{
- float val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
}
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
{
- int val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
}
bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
{
- bool val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
}
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+int getParameterStringVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<std::string>& val, int length)
{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+
+int getParameterStringVectorUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<std::string>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
}
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
}
+int getParameterFloatVectorUnnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val.size();
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
}
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
index 93f5a305abf457a3f457884c29998b656dedb357..748c5c374d5794b5c31974886337662eb49ca329 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
@@ -488,7 +488,11 @@ int main(int argc, char* argv[])
// FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
- // Call the class function that loads the parameters for this class.
+ // Call the class function that loads the parameters
+ // from the "BatteryMonitor.yaml" file.
+ // > This is possible because that YAML file is added
+ // to the parameter service when launched via the
+ // "agent.launch" file.
fetchBatteryMonitorYamlParameters(nodeHandle_to_own_agent_parameter_service);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index 02d6c2d39b53ad04c151f6d4eb44d5ec5de65a64..b80cd3bb064d64a18e7cd9c1c0827475b73d6b4e 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -1003,7 +1003,7 @@ void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > 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);
+ getParameterFloatVectorKnownLength(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");
@@ -1011,7 +1011,7 @@ void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle)
// 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);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
// Boolean indiciating whether the "Debug Message" of this agent
// should be published or not
@@ -1023,10 +1023,10 @@ void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle)
// 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);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
// Integrator gains
yaml_integratorGain_forThrust = getParameterFloat(nodeHandle_for_paramaters, "integratorGain_forThrust");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index b945d1230a11665607bb747824b76310746f86b2..8ad564c74af3906651d3ad4bbba1d3c753a1135e 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -1802,7 +1802,7 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > 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);
+ getParameterFloatVectorKnownLength(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");
@@ -1810,7 +1810,7 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// 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);
+ getParameterFloatVectorKnownLength(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");
@@ -1822,13 +1822,13 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
yaml_controller_method = getParameterInt( nodeHandle_for_paramaters , "controller_method" );
// The LQR Controller parameters for z-height
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_2StateVector", yaml_gainMatrixThrust_2StateVector, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_2StateVector", yaml_gainMatrixThrust_2StateVector, 2);
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_3StateVector", yaml_gainMatrixRollRate_3StateVector, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_3StateVector", yaml_gainMatrixPitchRate_3StateVector, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate_3StateVector", yaml_gainMatrixRollRate_3StateVector, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate_3StateVector", yaml_gainMatrixPitchRate_3StateVector, 3);
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_2StateVector", yaml_gainMatrixRollAngle_2StateVector, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle_2StateVector", yaml_gainMatrixPitchAngle_2StateVector, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle_2StateVector", yaml_gainMatrixRollAngle_2StateVector, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle_2StateVector", yaml_gainMatrixPitchAngle_2StateVector, 2);
yaml_gainRollRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainRollRate_fromAngle");
yaml_gainPitchRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainPitchRate_fromAngle");
// The LQR Controller parameters for yaw
@@ -1844,9 +1844,9 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp
index 7988825d0e6b7b62c58a5b4f8ff217bb0ad575ee..9dcffe20abfb7cf68bcda2ce9e192fe68f40e25c 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp
@@ -1526,7 +1526,7 @@ void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paraMaters, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paraMaters, "shouldPerformConvertIntoBodyFrame");
@@ -1565,36 +1565,36 @@ void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle)
estimator_method = getParameterInt( nodeHandle_for_paraMaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_MOTOR"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor1", gainMatrixMotor1, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor2", gainMatrixMotor2, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor3", gainMatrixMotor3, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor4", gainMatrixMotor4, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixMotor1", gainMatrixMotor1, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixMotor2", gainMatrixMotor2, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixMotor3", gainMatrixMotor3, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixMotor4", gainMatrixMotor4, 12);
// The LQR Controller parameters for "LQR_MODE_MOTOR"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_TwelveStateVector", gainMatrixThrust_TwelveStateVector, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollTorque", gainMatrixRollTorque, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchTorque", gainMatrixPitchTorque, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixYawTorque", gainMatrixYawTorque, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixThrust_TwelveStateVector", gainMatrixThrust_TwelveStateVector, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollTorque", gainMatrixRollTorque, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchTorque", gainMatrixPitchTorque, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixYawTorque", gainMatrixYawTorque, 12);
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// The parameters for the "Angle Reponse Test" controller mode
angleResponseTest_pitchAngle_degrees = getParameterFloat(nodeHandle_for_paraMaters, "angleResponseTest_pitchAngle_degrees");
@@ -1607,13 +1607,13 @@ void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle)
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
index f4dfe8e42d2f63d9f50564f0fdc6e775da885587..c7877f0f262797b515f782ee861012796d3e5b8f 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
@@ -230,7 +230,7 @@ void fetchMocapEmulatorConfigYamlParameters()
yaml_mocap_frequency = getParameterFloat(nodeHandle_for_paramaters,"mocap_frequency");
// THE COEFFICIENT OF THE 16-BIT COMMAND TO THRUST CONVERSION
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
// THE MIN AND MAX FOR SATURATING THE 16-BIT THRUST COMMANDS
yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp
index 9a0545e31edbb227f31deb314f7443a02fe59a6e..ec962cbb68d679a6fb2ae9ca00b2dfd20bf274d3 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp
@@ -223,7 +223,7 @@ void setpointCallback(const Setpoint& newSetpoint);
// LOAD PARAMETERS
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
@@ -718,26 +718,26 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_MpcController, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "motorPoly", motorPoly, 3);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[MPC CONTROLLER] DEBUGGING: the fetched CustomController/mass = " << cf_mass);
- getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// Call the function that computes details an values that are needed from these
@@ -780,7 +780,7 @@ float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
return val;
}
// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
ROS_ERROR_STREAM("missing parameter '" << name << "'");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
index 269f0a55fdacbe0a1c9219a8ec2eeedbe6074e90..4180b69d1ed5d062baa0da48388b3cba2bdeacef 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
@@ -76,84 +76,100 @@
bool requestLoadYamlFilenameCallbackServiceCallback(LoadYamlFromFilename::Request &request, LoadYamlFromFilename::Response &response)
{
- // Put the flying state into the response variable
- requestLoadYamlFilenamePublisher.publish(request.stringWithHeader);
+ // Put the flying state into the response variable
+ m_requestLoadYamlFilenamePublisher.publish(request.stringWithHeader);
- // Put success into the response
- response.data = 1;
+ // Put success into the response
+ response.data = 1;
- // Return
- return true;
+ // Return
+ return true;
}
void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_load_with_header)
{
- // LOAD THE YAML FILE
-
- // Get the yaml file name requested
- std::string yaml_filename_to_load = yaml_filename_to_load_with_header.data;
- // Get the node handle to this parameter service
- ros::NodeHandle nodeHandle("~");
- // Instantiate a local variable for the command string that will be passed to the "system":
- std::string cmd;
- // Get the abolute path to "dfall_pkg":
- std::string dfall_pkg_path = ros::package::getPath("dfall_pkg");
- // Construct the system command string for (re-)loading the parameters:
- cmd = "rosparam load " + dfall_pkg_path + "/param" + "/" + yaml_filename_to_load + ".yaml " + m_base_namespace + "/" + yaml_filename_to_load;
- // Let the user know what is about to happen
- ROS_INFO_STREAM("[PARAMETER SERVICE] The following path will be used for locating the .yaml file: " << dfall_pkg_path << " The comand line string sent to the 'system' is: " << cmd );
- // Send the "load yaml" command to the system
- system(cmd.c_str());
-
-
-
- // PUBLISH A MESSAGE THAT THE YAML FILE WAS LOADED
-
- // Create publisher as a local variable, using the filename
- // as the name of the message
- ros::Publisher yamlParametersReadyForFetchPublisher = nodeHandle.advertise<IntWithHeader>(yaml_filename_to_load, 1, true);
- //ros::spinOnce();
- // Create a local variable for the message
- IntWithHeader yaml_ready_msg;
- // Specify with the data the "type" of this parameter service
- switch (m_type)
- {
- case TYPE_AGENT:
- {
- yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
- break;
- }
- case TYPE_COORDINATOR:
- {
- yaml_ready_msg.data = LOAD_YAML_FROM_COORDINATOR;
- break;
- }
- default:
- {
- // Throw an error if the type is not recognised
- ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
- // Specify to load from the agent by default
- yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
- break;
- }
- }
- // Copy across the boolean field
- yaml_ready_msg.shouldCheckForAgentID = yaml_filename_to_load_with_header.shouldCheckForAgentID;
- // Copy across the vector of IDs
- if (yaml_filename_to_load_with_header.shouldCheckForAgentID)
- {
- for ( int i_ID=0 ; i_ID<yaml_filename_to_load_with_header.agentIDs.size() ; i_ID++ )
- {
- yaml_ready_msg.agentIDs.push_back(yaml_filename_to_load_with_header.agentIDs[i_ID]);
- }
- }
- // Sleep to make the publisher known to ROS (in seconds)
- ros::Duration(2.0).sleep();
- // Send the message
- yamlParametersReadyForFetchPublisher.publish(yaml_ready_msg);
-
- // Inform the user that this was published
- ROS_INFO_STREAM("[PARAMETER SERVICE] Published message that " << yaml_filename_to_load << " yaml parameters are ready." );
+ // LOAD THE YAML FILE
+
+ // Get the yaml file name requested
+ std::string yaml_filename_to_load = yaml_filename_to_load_with_header.data;
+ // Instantiate a local variable for the command string that will be passed to the "system":
+ std::string cmd;
+ // Get the abolute path to "dfall_pkg":
+ std::string dfall_pkg_path = ros::package::getPath("dfall_pkg");
+ // Construct the system command string for (re-)loading the parameters:
+ cmd = "rosparam load " + dfall_pkg_path + "/param" + "/" + yaml_filename_to_load + ".yaml " + m_base_namespace + "/" + yaml_filename_to_load;
+ // Let the user know what is about to happen
+ ROS_INFO_STREAM("[PARAMETER SERVICE] The following path will be used for locating the .yaml file: " << dfall_pkg_path << " The comand line string sent to the 'system' is: " << cmd );
+ // Send the "load yaml" command to the system
+ system(cmd.c_str());
+
+
+
+ // PREPARE A MESSAGE THAT THE YAML FILE WAS LOADED
+
+ // Create a local variable for the message
+ IntWithHeader yaml_ready_msg;
+ // Specify with the data the "type" of this parameter service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ yaml_ready_msg.data = LOAD_YAML_FROM_COORDINATOR;
+ break;
+ }
+ default:
+ {
+ // Throw an error if the type is not recognised
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
+ // Specify to load from the agent by default
+ yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ break;
+ }
+ }
+ // Copy across the boolean field
+ yaml_ready_msg.shouldCheckForAgentID = yaml_filename_to_load_with_header.shouldCheckForAgentID;
+ // Copy across the vector of IDs
+ if (yaml_filename_to_load_with_header.shouldCheckForAgentID)
+ {
+ for ( int i_ID=0 ; i_ID<yaml_filename_to_load_with_header.agentIDs.size() ; i_ID++ )
+ {
+ yaml_ready_msg.agentIDs.push_back(yaml_filename_to_load_with_header.agentIDs[i_ID]);
+ }
+ }
+
+ // PUBLISH A MESSAGE THAT THE YAML FILE WAS LOADED
+
+ // Check if the publisher is already in the "map"
+ if (m_yamlParametersReadyForFetchPublisherMap.count(yaml_filename_to_load) > 0)
+ {
+ // Send the message
+ m_yamlParametersReadyForFetchPublisherMap[yaml_filename_to_load].publish(yaml_ready_msg);
+ // Inform the user that this was published
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Published message that " << yaml_filename_to_load << " yaml parameters are ready." );
+ }
+ else
+ {
+ // Create a publisher
+ // > using the filename as the name of the message
+ // > adding it to the map of publihsers to make the
+ // next load faster
+ // Get the node handle to this parameter service
+ ros::NodeHandle nodeHandle("~");
+ m_yamlParametersReadyForFetchPublisherMap[yaml_filename_to_load] = nodeHandle.advertise<IntWithHeader>(yaml_filename_to_load, 1);
+ // Inform the user that this publisher was added
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Added " << yaml_filename_to_load << " to the publisher map." );
+ // Sleep to make the publisher known to ROS (in seconds)
+ ros::Duration(2.0).sleep();
+ // Send the message
+ m_yamlParametersReadyForFetchPublisherMap[yaml_filename_to_load].publish(yaml_ready_msg);
+ // Inform the user that this was published
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Published message that " << yaml_filename_to_load << " yaml parameters are ready." );
+ }
}
@@ -162,89 +178,89 @@ void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_lo
bool getTypeAndIDParameters()
{
- // Initialise the return variable as success
- bool return_was_successful = true;
-
- // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
- // the "~" indcates that "self" is the node handle assigned to this variable.
- ros::NodeHandle nodeHandle("~");
-
- // Get the value of the "type" parameter into a local string variable
- std::string type_string;
- if(!nodeHandle.getParam("type", type_string))
- {
- // Throw an error if the agent ID parameter could not be obtained
- ROS_ERROR("[PARAMETER SERVICE] Failed to get type");
- }
-
- // Set the "m_type" class variable based on this string loaded
- if ((!type_string.compare("coordinator")))
- {
- m_type = TYPE_COORDINATOR;
- }
- else if ((!type_string.compare("agent")))
- {
- m_type = TYPE_AGENT;
- }
- else
- {
- // Set "m_type" to the value indicating that it is invlid
- m_type = TYPE_INVALID;
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
- }
-
-
- // Construct the string to the namespace of this Paramater Service
- switch (m_type)
- {
- case TYPE_AGENT:
- {
- // Get the value of the "agentID" parameter into the class variable "m_Id"
- if(!nodeHandle.getParam("agentID", m_ID))
- {
- // Throw an error if the agent ID parameter could not be obtained
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] Failed to get agentID");
- }
- else
- {
- // Inform the user about the type and ID
- ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type AGENT with ID = " << m_ID);
- }
- break;
- }
-
- // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
- // > The master GUI
- case TYPE_COORDINATOR:
- {
- // Get the value of the "coordID" parameter into the class variable "m_Id"
- if(!nodeHandle.getParam("coordID", m_ID))
- {
- // Throw an error if the coord ID parameter could not be obtained
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] Failed to get coordID");
- }
- else
- {
- // Inform the user about the type and ID
- ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type COORDINATOR with ID = " << m_ID);
- }
- break;
- }
-
- default:
- {
- // Throw an error if the type is not recognised
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
- break;
- }
- }
-
- // Return
- return return_was_successful;
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the value of the "type" parameter into a local string variable
+ std::string type_string;
+ if(!nodeHandle.getParam("type", type_string))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
}
@@ -253,47 +269,47 @@ bool getTypeAndIDParameters()
bool constructNamespaces()
{
- // Initialise the return variable as success
- bool return_was_successful = true;
-
- // Get the namespace of this "ParameterService" node
- std::string this_node_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() = " << this_node_namespace);
-
- // Construct the string to the namespace of this Paramater Service
- switch (m_type)
- {
- case TYPE_AGENT:
- {
- //m_base_namespace = ros::this_node::getNamespace();
- //m_base_namespace = "/agent" + m_Id + '/' + "ParameterService";
- m_base_namespace = this_node_namespace + '/' + "ParameterService";
- ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
- break;
- }
-
- // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
- // > The master GUI
- case TYPE_COORDINATOR:
- {
- //m_base_namespace = ros::this_node::getNamespace();
- //m_base_namespace = "/ParameterService";
- m_base_namespace = this_node_namespace + '/' + "ParameterService";
- ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
- break;
- }
-
- default:
- {
- // Throw an error if the type is not recognised
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] The 'm_type' type parameter was not recognised.");
- break;
- }
- }
-
- // Return
- return return_was_successful;
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Get the namespace of this "ParameterService" node
+ std::string this_node_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() = " << this_node_namespace);
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/agent" + m_Id + '/' + "ParameterService";
+ m_base_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/ParameterService";
+ m_base_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' type parameter was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
}
@@ -310,40 +326,69 @@ bool constructNamespaces()
int main(int argc, char* argv[])
{
- // Starting the ROS-node
- ros::init(argc, argv, "ParameterService");
-
- // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
- // the "~" indcates that "self" is the node handle assigned to this variable.
- ros::NodeHandle nodeHandle("~");
-
- // Get the type and ID of this parameter service
- bool isValid_type_and_ID = getTypeAndIDParameters();
-
- // Construct the namespace into which this parameter service
- // loads yaml parameters
- bool isValid_namespaces = constructNamespaces();
-
- // Stall if the TYPE and ID are not valid
- if ( !( isValid_type_and_ID && isValid_namespaces ) )
- {
- ROS_ERROR("[PARAMETER SERVICE] Service NOT FUNCTIONING :-)");
- ros::spin();
- }
-
- // Subscribe to the messages that request loading a yaml file
- ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 20, requestLoadYamlFilenameCallback);
-
- // Publisher for publishing "internally" to the subscriber above
- requestLoadYamlFilenamePublisher = nodeHandle.advertise <StringWithHeader>("requestLoadYamlFilename", 1);
-
- // Advertise the service for requests to load a yaml file
- ros::ServiceServer requestLoadYamlFilenameService = nodeHandle.advertiseService("requestLoadYamlFilename", requestLoadYamlFilenameCallbackServiceCallback);
-
- // Inform the user the this node is ready
- ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
- // Spin as a single-thread node
- ros::spin();
-
- return 0;
+ // Starting the ROS-node
+ ros::init(argc, argv, "ParameterService");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the type and ID of this parameter service
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Construct the namespace into which this parameter service
+ // loads yaml parameters
+ bool isValid_namespaces = constructNamespaces();
+
+ // Stall if the TYPE and ID are not valid
+ if ( !( isValid_type_and_ID && isValid_namespaces ) )
+ {
+ ROS_ERROR("[PARAMETER SERVICE] Service NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+ // Fetch the YAML parameter that itself is the array of
+ // YAML file name strings
+ ros::NodeHandle nodeHandle_to_yamlFileNames(nodeHandle, "YamlFileNames");
+ if(!nodeHandle_to_yamlFileNames.getParam("filenames", yaml_filenames_provided))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get filenames");
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO_STREAM("[PARAMETER SERVICE] loaded " << yaml_filenames_provided.size() << " filenames for which publishers will be prepared.");
+ }
+
+
+ // For each for the filenames provided:
+ // > Create a publisher, using the filename as the name
+ // of the message
+ // > Add the publisher to a "map", i.e., to a dictionary
+ // > Inform the user that this publisher was added
+ for (std::string filename_str : yaml_filenames_provided)
+ {
+ m_yamlParametersReadyForFetchPublisherMap[filename_str] = nodeHandle.advertise<IntWithHeader>(filename_str, 1);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Added " << filename_str << " to the publisher map." );
+ }
+
+
+ // Subscribe to the messages that request loading a yaml file
+ ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 20, requestLoadYamlFilenameCallback);
+
+ // Publisher for publishing "internally" to the subscriber above
+ m_requestLoadYamlFilenamePublisher = nodeHandle.advertise <StringWithHeader>("requestLoadYamlFilename", 1);
+
+ // Advertise the service for requests to load a yaml file
+ ros::ServiceServer requestLoadYamlFilenameService = nodeHandle.advertiseService("requestLoadYamlFilename", requestLoadYamlFilenameCallbackServiceCallback);
+
+ // Inform the user the this node is ready
+ ROS_INFO("[PARAMETER SERVICE] 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/PickerControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
index d226c811050662bc8d7bd019bd8f0c8f7c77318f..10152881cb1d60fc8789a17a667954a89ede4ed6 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
@@ -1253,7 +1253,7 @@ void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > 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);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
yaml_shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paramaters, "shouldPerformConvertIntoBodyFrame");
@@ -1272,10 +1272,10 @@ void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
// 16-BIT COMMAND LIMITS
yaml_cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
@@ -1283,13 +1283,13 @@ void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", yaml_PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", yaml_PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , yaml_PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", yaml_PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", yaml_PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , yaml_PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
index be9b4ef6b3fb65b3bb23f6ee7c84de665f71687f..3dda302685c63c274fbb01c0c588739237d8c89e 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
@@ -1419,9 +1419,9 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
remoteConrtolSetpointFactor_z = getParameterFloat(nodeHandle_for_paramaters , "remoteConrtolSetpointFactor_z");
// LQR Gain matrix for tracking the angle commands from the Crazyflie
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_forRemoteControl", gainMatrixRollRate_forRemoteControl, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_forRemoteControl", gainMatrixPitchRate_forRemoteControl, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate_forRemoteControl", gainMatrixYawRate_forRemoteControl, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate_forRemoteControl", gainMatrixRollRate_forRemoteControl, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate_forRemoteControl", gainMatrixPitchRate_forRemoteControl, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate_forRemoteControl", gainMatrixYawRate_forRemoteControl, 3);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/default_viconObjectName_forRemote = " << default_viconObjectName_forRemote);
@@ -1446,7 +1446,7 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paramaters, "shouldPerformConvertIntoBodyFrame");
@@ -1467,24 +1467,24 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// 16-BIT COMMAND LIMITS
@@ -1494,13 +1494,13 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp
index e9f1c08cfd6265c2585b4c14244354f0069a6178..ea4aa64b401207346d478de2f9db40c37d5a9a99 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp
@@ -289,24 +289,24 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_safeController, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "motorPoly", motorPoly, 3);
// > The row of the LQR matrix that commands body frame roll rate
- getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixRoll", gainMatrixRoll, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "gainMatrixRoll", gainMatrixRoll, 9);
// > The row of the LQR matrix that commands body frame pitch rate
- getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixPitch", gainMatrixPitch, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "gainMatrixPitch", gainMatrixPitch, 9);
// > The row of the LQR matrix that commands body frame yaw rate
- getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixYaw", gainMatrixYaw, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "gainMatrixYaw", gainMatrixYaw, 9);
// > The row of the LQR matrix that commands thrust adjustment
- getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixThrust", gainMatrixThrust, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "gainMatrixThrust", gainMatrixThrust, 9);
// > The gains for the point-mass Kalman filter
- getParameterFloatVector(nodeHandle_for_safeController, "filterGain", filterGain, 6);
- getParameterFloatVector(nodeHandle_for_safeController, "estimatorMatrix", estimatorMatrix, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "filterGain", filterGain, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "estimatorMatrix", estimatorMatrix, 2);
// > The defailt setpoint of the controller
- getParameterFloatVector(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[SAFE CONTROLLER] DEBUGGING: the fetched SafeController/mass = " << cf_mass);
@@ -346,7 +346,7 @@ float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
return val;
}
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
ROS_ERROR_STREAM("missing parameter '" << name << "'");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
index c4619227eb64d57c2e6940996e7bd689490ebafa..64f37fda36232e23772470720353c701150029d0 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -850,7 +850,7 @@ void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle)
// 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);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
index 643912b07b86fd4dd7a3fd3da147e4e46b2e0156..c7a1952536734c9b7c8a51fd2aa2d4a88bc9502a 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
@@ -769,7 +769,7 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > 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);
+ getParameterFloatVectorKnownLength(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");
@@ -777,7 +777,7 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// 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);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
// Boolean indiciating whether the "Debug Message" of this agent
// should be published or not
@@ -789,10 +789,10 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// 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);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(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
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
index bf0a2a971f6433710b222c0c1a548cbb646c9f82..7ddc0d554d973a3342ec65e3c4839041836b9f49 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
@@ -1514,20 +1514,20 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
// PARAMETERS SPECIFIC TO THE TUNING CONTROL FEATURE
/// Setpoint for the HORIZONTAL test
- getParameterFloatVector(nodeHandle_for_paramaters, "test_horizontal_setpoint1", test_horizontal_setpoint1, 4);
- getParameterFloatVector(nodeHandle_for_paramaters, "test_horizontal_setpoint2", test_horizontal_setpoint2, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_horizontal_setpoint1", test_horizontal_setpoint1, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_horizontal_setpoint2", test_horizontal_setpoint2, 4);
// Setpoint for the VERTICAL test
- getParameterFloatVector(nodeHandle_for_paramaters, "test_vertical_setpoint1", test_vertical_setpoint1, 4);
- getParameterFloatVector(nodeHandle_for_paramaters, "test_vertical_setpoint2", test_vertical_setpoint2, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_vertical_setpoint1", test_vertical_setpoint1, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_vertical_setpoint2", test_vertical_setpoint2, 4);
// Setpoint for the HEADING test
- getParameterFloatVector(nodeHandle_for_paramaters, "test_heading_setpoint1", test_heading_setpoint1, 4);
- getParameterFloatVector(nodeHandle_for_paramaters, "test_heading_setpoint2", test_heading_setpoint2, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_heading_setpoint1", test_heading_setpoint1, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_heading_setpoint2", test_heading_setpoint2, 4);
// Setpoint for the ALL test
- getParameterFloatVector(nodeHandle_for_paramaters, "test_all_setpoint1", test_all_setpoint1, 4);
- getParameterFloatVector(nodeHandle_for_paramaters, "test_all_setpoint2", test_all_setpoint2, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_all_setpoint1", test_all_setpoint1, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_all_setpoint2", test_all_setpoint2, 4);
// Parameters for flying in a circle
test_circle_radius = getParameterFloat(nodeHandle_for_paramaters, "test_circle_radius");
@@ -1569,7 +1569,7 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paramaters, "shouldPerformConvertIntoBodyFrame");
@@ -1590,24 +1590,24 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// 16-BIT COMMAND LIMITS
@@ -1617,13 +1617,13 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded