From 5218ac59d8d96836c63b4d648735900a8c897d2b Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Mon, 13 Apr 2020 12:45:32 +0200
Subject: [PATCH] Updated the Parameter Service node so that that yaml files
are added to a map of Publishers and hence the sleep is only needed the first
time an unknown yaml file is loaded. Changed the getParameterFloatVector
function to be getParameterFloatVectorKnownLength. Added functionality to the
GUI to hide and show the load YAML bar.
---
.../forms/enablecontrollerloadyamlbar.ui | 817 ++++++++++--------
.../GUI_Qt/flyingAgentGUI/forms/mainwindow.ui | 13 +-
.../include/enablecontrollerloadyamlbar.h | 6 +
.../flyingAgentGUI/include/mainwindow.h | 3 +-
.../src/enablecontrollerloadyamlbar.cpp | 18 +
.../GUI_Qt/flyingAgentGUI/src/mainwindow.cpp | 26 +-
.../systemConfigGUI/include/mainguiwindow.h | 2 +-
.../classes/GetParamtersAndNamespaces.h | 8 +-
.../include/nodes/ParameterService.h | 11 +-
.../include/nodes/SafeControllerService.h | 2 +-
dfall_ws/src/dfall_pkg/launch/agent.launch | 5 +
.../src/dfall_pkg/param/YamlFileNames.yaml | 22 +-
.../src/classes/GetParamtersAndNamespaces.cpp | 106 ++-
.../dfall_pkg/src/nodes/BatteryMonitor.cpp | 6 +-
.../src/nodes/CsoneControllerService.cpp | 12 +-
.../src/nodes/DefaultControllerService.cpp | 20 +-
.../src/nodes/DemoControllerService.cpp | 56 +-
.../src/dfall_pkg/src/nodes/MocapEmulator.cpp | 2 +-
.../src/nodes/MpcControllerService.cpp | 24 +-
.../dfall_pkg/src/nodes/ParameterService.cpp | 509 ++++++-----
.../src/nodes/PickerControllerService.cpp | 22 +-
.../src/nodes/RemoteControllerService.cpp | 46 +-
.../src/nodes/SafeControllerService.cpp | 18 +-
.../src/nodes/StudentControllerService.cpp | 2 +-
.../src/nodes/TemplateControllerService.cpp | 12 +-
.../src/nodes/TuningControllerService.cpp | 56 +-
26 files changed, 1008 insertions(+), 816 deletions(-)
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 d92fc08b..f52ce5f9 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 aa37362f..401d1042 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 c411e2d1..888f6cad 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 09cab557..63a0e376 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 8fc5ca82..31c95bdb 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 c29b8323..e61244c2 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 5d17177c..9b4cfae5 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 5c9f4339..fb351252 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 d298e9e2..bed20975 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 3a540f29..f7c666e3 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 fe0977ca..c3212905 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 6e9de18c..94ce721d 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 c299932b..63e50c81 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 93f5a305..748c5c37 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 02d6c2d3..b80cd3bb 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 b945d123..8ad564c7 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 7988825d..9dcffe20 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 f4dfe8e4..c7877f0f 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 9a0545e3..ec962cbb 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 269f0a55..4180b69d 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 d226c811..10152881 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 be9b4ef6..3dda3026 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 e9f1c08c..ea4aa64b 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 c4619227..64f37fda 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 643912b0..c7a19525 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 bf0a2a97..7ddc0d55 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
--
GitLab