From 23147cfdece3f4948da07cf12b92152175fba605 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Tue, 19 Mar 2024 10:51:43 -0600 Subject: [PATCH 01/21] initial gui file --- rosplane_tuning/gui/tuning_gui.ui | 228 ++++++++++++++++++++++++++++++ 1 file changed, 228 insertions(+) create mode 100644 rosplane_tuning/gui/tuning_gui.ui diff --git a/rosplane_tuning/gui/tuning_gui.ui b/rosplane_tuning/gui/tuning_gui.ui new file mode 100644 index 0000000..4128e02 --- /dev/null +++ b/rosplane_tuning/gui/tuning_gui.ui @@ -0,0 +1,228 @@ + + + MainWindow + + + + 0 + 0 + 810 + 600 + + + + + 0 + 0 + + + + MainWindow + + + + + 0 + 0 + + + + + + 20 + 10 + 761 + 571 + + + + + 6 + + + QLayout::SetFixedSize + + + 0 + + + + + 6 + + + QLayout::SetMaximumSize + + + 50 + + + + + Course + + + + + + + Roll + + + + + + + Pitch + + + + + + + Airspeed + + + + + + + Altitude + + + + + + + Run + + + + + + + Clear + + + + + + + + + + + + + k_p + + + + + + + k_i + + + + + + + k_d + + + + + + + + + QLayout::SetDefaultConstraint + + + 0 + + + 0 + + + + + + 0 + 0 + + + + + 400 + 50 + + + + Qt::Horizontal + + + + + + + + 0 + 0 + + + + + 400 + 50 + + + + Qt::Horizontal + + + + + + + + 0 + 0 + + + + + 400 + 50 + + + + Qt::Horizontal + + + + + + + + + + + + + + + + + + + + + + + + + + + From 464164122efa01b3809dd249da2e10c8d040574a Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Tue, 19 Mar 2024 11:15:21 -0600 Subject: [PATCH 02/21] python file initial commit --- rosplane_tuning/gui/tuning_functionality.py | 12 ++ rosplane_tuning/gui/tuning_gui.py | 144 ++++++++++++++++++++ 2 files changed, 156 insertions(+) create mode 100644 rosplane_tuning/gui/tuning_functionality.py create mode 100644 rosplane_tuning/gui/tuning_gui.py diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py new file mode 100644 index 0000000..da1baee --- /dev/null +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -0,0 +1,12 @@ +import sys +from PyQt5.QtCore import Qt +from PyQt5.QtWidgets import * +from PyQt5.QtWidgets import QWidget +from PyQt5.uic import loadUi +from tuning_gui import Ui_MainWindow + +class Window(QMainWindow, Ui_MainWindow): + def __init__(self, parent: QWidget | None = ..., flags: Qt.WindowFlags | Qt.WindowType = ...) -> None: + super().__init__(parent, flags) + self.setupUi(self) + self.connectSignalSlots() \ No newline at end of file diff --git a/rosplane_tuning/gui/tuning_gui.py b/rosplane_tuning/gui/tuning_gui.py new file mode 100644 index 0000000..2826927 --- /dev/null +++ b/rosplane_tuning/gui/tuning_gui.py @@ -0,0 +1,144 @@ +# -*- coding: utf-8 -*- + +# Form implementation generated from reading ui file 'tuning_gui.ui' +# +# Created by: PyQt5 UI code generator 5.15.9 +# +# WARNING: Any manual changes made to this file will be lost when pyuic5 is +# run again. Do not edit this file unless you know what you are doing. + + +from PyQt5 import QtCore, QtGui, QtWidgets + + +class Ui_MainWindow(object): + def setupUi(self, MainWindow): + MainWindow.setObjectName("MainWindow") + MainWindow.resize(810, 600) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Fixed, QtWidgets.QSizePolicy.Fixed) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(MainWindow.sizePolicy().hasHeightForWidth()) + MainWindow.setSizePolicy(sizePolicy) + self.centralwidget = QtWidgets.QWidget(MainWindow) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Preferred, QtWidgets.QSizePolicy.Preferred) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.centralwidget.sizePolicy().hasHeightForWidth()) + self.centralwidget.setSizePolicy(sizePolicy) + self.centralwidget.setObjectName("centralwidget") + self.horizontalLayoutWidget = QtWidgets.QWidget(self.centralwidget) + self.horizontalLayoutWidget.setGeometry(QtCore.QRect(20, 10, 761, 571)) + self.horizontalLayoutWidget.setObjectName("horizontalLayoutWidget") + self.horizontalLayout = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget) + self.horizontalLayout.setSizeConstraint(QtWidgets.QLayout.SetFixedSize) + self.horizontalLayout.setContentsMargins(0, 0, 0, 0) + self.horizontalLayout.setSpacing(6) + self.horizontalLayout.setObjectName("horizontalLayout") + self.verticalLayout = QtWidgets.QVBoxLayout() + self.verticalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize) + self.verticalLayout.setContentsMargins(-1, -1, 50, -1) + self.verticalLayout.setSpacing(6) + self.verticalLayout.setObjectName("verticalLayout") + self.CourseButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) + self.CourseButton.setObjectName("CourseButton") + self.verticalLayout.addWidget(self.CourseButton) + self.rollButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) + self.rollButton.setObjectName("rollButton") + self.verticalLayout.addWidget(self.rollButton) + self.pitchButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) + self.pitchButton.setObjectName("pitchButton") + self.verticalLayout.addWidget(self.pitchButton) + self.airspeedButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) + self.airspeedButton.setObjectName("airspeedButton") + self.verticalLayout.addWidget(self.airspeedButton) + self.altitudeButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) + self.altitudeButton.setObjectName("altitudeButton") + self.verticalLayout.addWidget(self.altitudeButton) + self.runButton = QtWidgets.QPushButton(self.horizontalLayoutWidget) + self.runButton.setObjectName("runButton") + self.verticalLayout.addWidget(self.runButton) + self.Clear = QtWidgets.QPushButton(self.horizontalLayoutWidget) + self.Clear.setObjectName("Clear") + self.verticalLayout.addWidget(self.Clear) + self.horizontalLayout.addLayout(self.verticalLayout) + self.horizontalLayout_3 = QtWidgets.QHBoxLayout() + self.horizontalLayout_3.setObjectName("horizontalLayout_3") + self.verticalLayout_4 = QtWidgets.QVBoxLayout() + self.verticalLayout_4.setObjectName("verticalLayout_4") + self.label_2 = QtWidgets.QLabel(self.horizontalLayoutWidget) + self.label_2.setObjectName("label_2") + self.verticalLayout_4.addWidget(self.label_2) + self.label = QtWidgets.QLabel(self.horizontalLayoutWidget) + self.label.setObjectName("label") + self.verticalLayout_4.addWidget(self.label) + self.label_3 = QtWidgets.QLabel(self.horizontalLayoutWidget) + self.label_3.setObjectName("label_3") + self.verticalLayout_4.addWidget(self.label_3) + self.horizontalLayout_3.addLayout(self.verticalLayout_4) + self.verticalLayout_3 = QtWidgets.QVBoxLayout() + self.verticalLayout_3.setSizeConstraint(QtWidgets.QLayout.SetDefaultConstraint) + self.verticalLayout_3.setContentsMargins(0, -1, 0, -1) + self.verticalLayout_3.setObjectName("verticalLayout_3") + self.kpSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.kpSlider.sizePolicy().hasHeightForWidth()) + self.kpSlider.setSizePolicy(sizePolicy) + self.kpSlider.setMaximumSize(QtCore.QSize(400, 50)) + self.kpSlider.setOrientation(QtCore.Qt.Horizontal) + self.kpSlider.setObjectName("kpSlider") + self.verticalLayout_3.addWidget(self.kpSlider) + self.kiSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.kiSlider.sizePolicy().hasHeightForWidth()) + self.kiSlider.setSizePolicy(sizePolicy) + self.kiSlider.setMaximumSize(QtCore.QSize(400, 50)) + self.kiSlider.setOrientation(QtCore.Qt.Horizontal) + self.kiSlider.setObjectName("kiSlider") + self.verticalLayout_3.addWidget(self.kiSlider) + self.kdSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) + sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.kdSlider.sizePolicy().hasHeightForWidth()) + self.kdSlider.setSizePolicy(sizePolicy) + self.kdSlider.setMaximumSize(QtCore.QSize(400, 50)) + self.kdSlider.setOrientation(QtCore.Qt.Horizontal) + self.kdSlider.setObjectName("kdSlider") + self.verticalLayout_3.addWidget(self.kdSlider) + self.horizontalLayout_3.addLayout(self.verticalLayout_3) + self.horizontalLayout.addLayout(self.horizontalLayout_3) + self.verticalLayout_5 = QtWidgets.QVBoxLayout() + self.verticalLayout_5.setObjectName("verticalLayout_5") + self.kpSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) + self.kpSpinBox.setObjectName("kpSpinBox") + self.verticalLayout_5.addWidget(self.kpSpinBox) + self.kiSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) + self.kiSpinBox.setObjectName("kiSpinBox") + self.verticalLayout_5.addWidget(self.kiSpinBox) + self.kdSpinBox_2 = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) + self.kdSpinBox_2.setObjectName("kdSpinBox_2") + self.verticalLayout_5.addWidget(self.kdSpinBox_2) + self.horizontalLayout.addLayout(self.verticalLayout_5) + MainWindow.setCentralWidget(self.centralwidget) + + self.retranslateUi(MainWindow) + QtCore.QMetaObject.connectSlotsByName(MainWindow) + + def retranslateUi(self, MainWindow): + _translate = QtCore.QCoreApplication.translate + MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow")) + self.CourseButton.setText(_translate("MainWindow", "Course")) + self.rollButton.setText(_translate("MainWindow", "Roll")) + self.pitchButton.setText(_translate("MainWindow", "Pitch")) + self.airspeedButton.setText(_translate("MainWindow", "Airspeed")) + self.altitudeButton.setText(_translate("MainWindow", "Altitude")) + self.runButton.setText(_translate("MainWindow", "Run")) + self.Clear.setText(_translate("MainWindow", "Clear")) + self.label_2.setText(_translate("MainWindow", "k_p")) + self.label.setText(_translate("MainWindow", "k_i")) + self.label_3.setText(_translate("MainWindow", "k_d")) From 2352b832dc822ddd2f4266242decb3b7b57be515 Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Tue, 19 Mar 2024 12:29:56 -0600 Subject: [PATCH 03/21] we wrote a draft of the tuning UI --- rosplane_tuning/gui/tuning_functionality.py | 88 ++++++++++++++++++++- 1 file changed, 87 insertions(+), 1 deletion(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index da1baee..7e79e6c 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -4,9 +4,95 @@ from PyQt5.QtWidgets import QWidget from PyQt5.uic import loadUi from tuning_gui import Ui_MainWindow +import subprocess class Window(QMainWindow, Ui_MainWindow): def __init__(self, parent: QWidget | None = ..., flags: Qt.WindowFlags | Qt.WindowType = ...) -> None: super().__init__(parent, flags) self.setupUi(self) - self.connectSignalSlots() \ No newline at end of file + self.connectSignalSlots() + + # Object Attributes that we use, with initializations + self.tuning_mode = '' + self.curr_kp = 0.0 + self.curr_kd = 0.0 + self.curr_ki = 0.0 + + def connectSignalSlots(self): + # This is where we define signal slots (callbacks) for when the buttons get clicked + self.CourseButton.toggled.connect(self.courseButtonCallback) + self.pitchButton.toggled.connect(self.pitchButtonCallback) + self.rollButton.toggled.connect(self.rollButtonCallback) + self.airspeedButton.toggled.connect(self.airspeedButtonCallback) + self.altitudeButton.toggled.connect(self.altitudeButtonCallback) + self.runButton.clicked.connect(self.runButtonCallback) + #self.slider.valueChanged.connect(self.slider_callback) + + def courseButtonCallback(self): + # Set the tuning mode + self.tuning_mode = 'c' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + self.set_sliders(self) + + def rollButtonCallback(self): + # Set the tuning mode + self.tuning_mode = 'r' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + + def pitchButtonCallback(self): + # Set the tuning mode + self.tuning_mode = 'p' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + + def airspeedButtonCallback(self): + # Set the tuning mode + self.tuning_mode = 'a_t' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + + def altitudeButtonCallback(self): + # Set the tuning mode + self.tuning_mode = 'a' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + + def get_param_output(self, param:str) -> float: + output = subprocess.run(["ros2","param","get","/autopilot",f"{self.tuning_mode}_k{param}"], stdout=subprocess.PIPE) + output_list = output.stdout.split() + output_val = output_list[-1] + return float(output_val) + + def set_sliders(self): + self.kpSlider.setMinimum(0) + self.kpSlider.setMaximum(100) + self.kiSlider.setMinimum(0) + self.kiSlider.setMaximum(100) + self.kdSlider.setMinimum(0) + self.kdSlider.setMaximum(100) + + #slider stuff + #self.slider.valueChanged.connect(self.slider_callback) + #put somewhere else "self.runButton.clicked.connect(self.runButtonCallback)"" + + def runButtonCallback(self): + #call this if run button is pushed + #execute ros param set functions + executable1 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_kp", f"{self.curr_kp}"] + executable2 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_ki", f"{self.curr_ki}"] + executable3 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_kd", f"{self.curr_kd}"] + executables = [executable1, executable2, executable3] + for executable in executables: + subprocess.run(executable) \ No newline at end of file From d310d27a42187079de1e763df0a8a88db32b699c Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Wed, 20 Mar 2024 11:10:02 -0600 Subject: [PATCH 04/21] updates to sliders --- rosplane_tuning/gui/tuning_functionality.py | 72 +++++++++++++++++---- rosplane_tuning/gui/tuning_gui.py | 2 +- 2 files changed, 62 insertions(+), 12 deletions(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 7e79e6c..5a49273 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -5,10 +5,11 @@ from PyQt5.uic import loadUi from tuning_gui import Ui_MainWindow import subprocess +import timeit class Window(QMainWindow, Ui_MainWindow): - def __init__(self, parent: QWidget | None = ..., flags: Qt.WindowFlags | Qt.WindowType = ...) -> None: - super().__init__(parent, flags) + def __init__(self): #, parent: QWidget | None = ..., flags: Qt.WindowFlags | Qt.WindowType = ...) -> None: + super().__init__() #parent, flags) self.setupUi(self) self.connectSignalSlots() @@ -17,6 +18,13 @@ def __init__(self, parent: QWidget | None = ..., flags: Qt.WindowFlags | Qt.Wind self.curr_kp = 0.0 self.curr_kd = 0.0 self.curr_ki = 0.0 + # This allows us to have different ranges for fine tuning kp, ki, and kd + self.kp_edit_dist = 2.0 + self.ki_edit_dist = 0.5 + self.kd_edit_dist = 2.0 + # Boolean values for controlling debugging statements + self.time = False + self.disp = True def connectSignalSlots(self): # This is where we define signal slots (callbacks) for when the buttons get clicked @@ -26,7 +34,9 @@ def connectSignalSlots(self): self.airspeedButton.toggled.connect(self.airspeedButtonCallback) self.altitudeButton.toggled.connect(self.altitudeButtonCallback) self.runButton.clicked.connect(self.runButtonCallback) - #self.slider.valueChanged.connect(self.slider_callback) + self.kpSlider.sliderReleased.connect(self.kp_slider_callback) + self.kiSlider.sliderReleased.connect(self.ki_slider_callback) + self.kdSlider.sliderReleased.connect(self.kd_slider_callback) def courseButtonCallback(self): # Set the tuning mode @@ -35,7 +45,8 @@ def courseButtonCallback(self): self.curr_kp = self.get_param_output('p') self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') - self.set_sliders(self) + # Set the sliders to the appropriate values + self.set_sliders() def rollButtonCallback(self): # Set the tuning mode @@ -44,6 +55,7 @@ def rollButtonCallback(self): self.curr_kp = self.get_param_output('p') self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') + self.set_sliders() def pitchButtonCallback(self): # Set the tuning mode @@ -52,6 +64,7 @@ def pitchButtonCallback(self): self.curr_kp = self.get_param_output('p') self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') + self.set_sliders() def airspeedButtonCallback(self): # Set the tuning mode @@ -60,6 +73,7 @@ def airspeedButtonCallback(self): self.curr_kp = self.get_param_output('p') self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') + self.set_sliders() def altitudeButtonCallback(self): # Set the tuning mode @@ -68,20 +82,48 @@ def altitudeButtonCallback(self): self.curr_kp = self.get_param_output('p') self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') + self.set_sliders() def get_param_output(self, param:str) -> float: + if self.time: start = timeit.timeit() output = subprocess.run(["ros2","param","get","/autopilot",f"{self.tuning_mode}_k{param}"], stdout=subprocess.PIPE) - output_list = output.stdout.split() - output_val = output_list[-1] - return float(output_val) + try: + output_list = output.stdout.split() + output_val = output_list[-1] + if self.time: print(timeit.timeit() - start) + return float(output_val) + except IndexError: + print("Error accessing ROS service output! Output:", output.stdout) + return -1 def set_sliders(self): - self.kpSlider.setMinimum(0) + # Sliders have an integer range. Set this from +- 100 + self.kpSlider.setValue(0) + self.kpSlider.setMinimum(-100) self.kpSlider.setMaximum(100) - self.kiSlider.setMinimum(0) + + self.kiSlider.setValue(0) + self.kiSlider.setMinimum(-100) self.kiSlider.setMaximum(100) - self.kdSlider.setMinimum(0) + + self.kdSlider.setValue(0) + self.kdSlider.setMinimum(-100) self.kdSlider.setMaximum(100) + + def kp_slider_callback(self): + slider_val = self.kpSlider.value() + self.curr_kp = self.curr_kp + self.kp_edit_dist * 100 / slider_val + if self.disp: print(self.curr_kp) + + def ki_slider_callback(self): + slider_val = self.kiSlider.value() + self.curr_ki = self.curr_ki + self.ki_edit_dist * 100 / slider_val + if self.disp: print(self.curr_ki) + + def kd_slider_callback(self): + slider_val = self.kdSlider.value() + self.curr_kd = self.curr_kd + self.kd_edit_dist * 100 / slider_val + if self.disp: print(self.curr_kd) #slider stuff #self.slider.valueChanged.connect(self.slider_callback) @@ -95,4 +137,12 @@ def runButtonCallback(self): executable3 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_kd", f"{self.curr_kd}"] executables = [executable1, executable2, executable3] for executable in executables: - subprocess.run(executable) \ No newline at end of file + subprocess.run(executable) + + +# Main loop +if __name__=='__main__': + app = QApplication(sys.argv) + window = Window() + window.show() + sys.exit(app.exec_()) diff --git a/rosplane_tuning/gui/tuning_gui.py b/rosplane_tuning/gui/tuning_gui.py index 2826927..0719256 100644 --- a/rosplane_tuning/gui/tuning_gui.py +++ b/rosplane_tuning/gui/tuning_gui.py @@ -131,7 +131,7 @@ def setupUi(self, MainWindow): def retranslateUi(self, MainWindow): _translate = QtCore.QCoreApplication.translate - MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow")) + MainWindow.setWindowTitle(_translate("MainWindow", "Rosplane Tuning GUI")) self.CourseButton.setText(_translate("MainWindow", "Course")) self.rollButton.setText(_translate("MainWindow", "Roll")) self.pitchButton.setText(_translate("MainWindow", "Pitch")) From a8fa011a61b8dfca009d628b5fafcdd56269c993 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Wed, 20 Mar 2024 11:35:55 -0600 Subject: [PATCH 05/21] slider value edits --- rosplane_tuning/gui/tuning_functionality.py | 36 ++++++++++++++++----- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 5a49273..5f77a9d 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -18,6 +18,7 @@ def __init__(self): #, parent: QWidget | None = ..., flags: Qt.WindowFlags | Qt. self.curr_kp = 0.0 self.curr_kd = 0.0 self.curr_ki = 0.0 + self.initialize_temps() # This allows us to have different ranges for fine tuning kp, ki, and kd self.kp_edit_dist = 2.0 self.ki_edit_dist = 0.5 @@ -25,6 +26,11 @@ def __init__(self): #, parent: QWidget | None = ..., flags: Qt.WindowFlags | Qt. # Boolean values for controlling debugging statements self.time = False self.disp = True + + def initialize_temps(self): + self.temp_kp = 0.0 + self.temp_kd = 0.0 + self.temp_ki = 0.0 def connectSignalSlots(self): # This is where we define signal slots (callbacks) for when the buttons get clicked @@ -47,6 +53,7 @@ def courseButtonCallback(self): self.curr_ki = self.get_param_output('i') # Set the sliders to the appropriate values self.set_sliders() + self.initialize_temps() def rollButtonCallback(self): # Set the tuning mode @@ -56,6 +63,7 @@ def rollButtonCallback(self): self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') self.set_sliders() + self.initialize_temps() def pitchButtonCallback(self): # Set the tuning mode @@ -65,6 +73,7 @@ def pitchButtonCallback(self): self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') self.set_sliders() + self.initialize_temps() def airspeedButtonCallback(self): # Set the tuning mode @@ -74,6 +83,7 @@ def airspeedButtonCallback(self): self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') self.set_sliders() + self.initialize_temps() def altitudeButtonCallback(self): # Set the tuning mode @@ -83,6 +93,7 @@ def altitudeButtonCallback(self): self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') self.set_sliders() + self.initialize_temps() def get_param_output(self, param:str) -> float: if self.time: start = timeit.timeit() @@ -112,18 +123,18 @@ def set_sliders(self): def kp_slider_callback(self): slider_val = self.kpSlider.value() - self.curr_kp = self.curr_kp + self.kp_edit_dist * 100 / slider_val - if self.disp: print(self.curr_kp) + self.temp_kp = self.curr_kp + self.kp_edit_dist * slider_val / 100 + if self.disp: print(self.temp_kp) def ki_slider_callback(self): slider_val = self.kiSlider.value() - self.curr_ki = self.curr_ki + self.ki_edit_dist * 100 / slider_val - if self.disp: print(self.curr_ki) - + self.temp_ki = self.curr_ki + self.ki_edit_dist * slider_val / 100 + if self.disp: print(self.temp_ki) + def kd_slider_callback(self): slider_val = self.kdSlider.value() - self.curr_kd = self.curr_kd + self.kd_edit_dist * 100 / slider_val - if self.disp: print(self.curr_kd) + self.temp_kd = self.curr_kd + self.kd_edit_dist * slider_val / 100 + if self.disp: print(slider_val, self.temp_kd) #slider stuff #self.slider.valueChanged.connect(self.slider_callback) @@ -131,13 +142,22 @@ def kd_slider_callback(self): def runButtonCallback(self): #call this if run button is pushed + # Set current variables to be temp variables + self.curr_kp = self.temp_kp + self.curr_ki = self.temp_ki + self.curr_kd = self.temp_kd #execute ros param set functions executable1 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_kp", f"{self.curr_kp}"] executable2 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_ki", f"{self.curr_ki}"] executable3 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_kd", f"{self.curr_kd}"] executables = [executable1, executable2, executable3] for executable in executables: - subprocess.run(executable) + output = subprocess.run(executable, stdout=subprocess.PIPE) + if self.disp: print(output.stdout) + if self.disp: + print('Kp set to:', self.curr_kp) + print('Ki set to:', self.curr_ki) + print('Kd set to:', self.curr_kd) # Main loop From ba17269151dd67b52f5d3c208f1799b8b27264f2 Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Wed, 20 Mar 2024 11:55:21 -0600 Subject: [PATCH 06/21] gui updates including functions to update spinboxes and slider values --- rosplane_tuning/gui/tuning_functionality.py | 24 +++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 5f77a9d..fe49424 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -125,16 +125,40 @@ def kp_slider_callback(self): slider_val = self.kpSlider.value() self.temp_kp = self.curr_kp + self.kp_edit_dist * slider_val / 100 if self.disp: print(self.temp_kp) + self.kpSpinBox.setValue(self.temp_kp) def ki_slider_callback(self): slider_val = self.kiSlider.value() self.temp_ki = self.curr_ki + self.ki_edit_dist * slider_val / 100 if self.disp: print(self.temp_ki) + self.kiSpinBox.setValue(self.temp_ki) def kd_slider_callback(self): slider_val = self.kdSlider.value() self.temp_kd = self.curr_kd + self.kd_edit_dist * slider_val / 100 if self.disp: print(slider_val, self.temp_kd) + self.kdSpinBox.setValue(self.temp_kd) + + + + def kpSpinBox_callback(self): + kpSpinBox_value = self.kpSpinBox.value() + self.temp_kp = kpSpinBox_value + slider_val = self.temp_kp*100/self.kp_edit_dist + self.kpSlider.setValue(slider_val) + + def kiSpinBox_callback(self): + kiSpinBox_value = self.kiSpinBox.value() + self.temp_ki = kiSpinBox_value + slider_val = self.temp_ki*100/self.ki_edit_dist + self.kiSlider.setValue(slider_val) + + def kdSpinBox_callback(self): + kdSpinBox_value = self.kdSpinBox.value() + self.temp_kd = kdSpinBox_value + slider_val = self.temp_kd*100/self.kd_edit_dist + self.kdSlider.setValue(slider_val) + #slider stuff #self.slider.valueChanged.connect(self.slider_callback) From 7476cd0670e8861ca9397e45961fb03c32c59ba6 Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Wed, 20 Mar 2024 12:00:16 -0600 Subject: [PATCH 07/21] gui updates including setspinboxes --- rosplane_tuning/gui/tuning_functionality.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index fe49424..16d7aef 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -53,6 +53,7 @@ def courseButtonCallback(self): self.curr_ki = self.get_param_output('i') # Set the sliders to the appropriate values self.set_sliders() + self.set_SpinBoxes() self.initialize_temps() def rollButtonCallback(self): @@ -63,6 +64,7 @@ def rollButtonCallback(self): self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') self.set_sliders() + self.set_SpinBoxes() self.initialize_temps() def pitchButtonCallback(self): @@ -73,6 +75,7 @@ def pitchButtonCallback(self): self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') self.set_sliders() + self.set_SpinBoxes() self.initialize_temps() def airspeedButtonCallback(self): @@ -83,6 +86,7 @@ def airspeedButtonCallback(self): self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') self.set_sliders() + self.set_SpinBoxes() self.initialize_temps() def altitudeButtonCallback(self): @@ -93,6 +97,7 @@ def altitudeButtonCallback(self): self.curr_kd = self.get_param_output('d') self.curr_ki = self.get_param_output('i') self.set_sliders() + self.set_SpinBoxes() self.initialize_temps() def get_param_output(self, param:str) -> float: @@ -120,6 +125,21 @@ def set_sliders(self): self.kdSlider.setValue(0) self.kdSlider.setMinimum(-100) self.kdSlider.setMaximum(100) + + + def set_SpinBoxes(self): + # Sliders have an integer range. Set this from +- 100 + self.kpSpinBox.setValue(0) + self.kpSpinBox.setMinimum(-100) + self.kpSpinBox.setMaximum(100) + + self.kiSpinBox.setValue(0) + self.kiSpinBox.setMinimum(-100) + self.kiSpinBox.setMaximum(100) + + self.kdSpinBox.setValue(0) + self.kdSpinBox.setMinimum(-100) + self.kdSpinBox.setMaximum(100) def kp_slider_callback(self): slider_val = self.kpSlider.value() From 66b65e027a70f220cccb2ca7ceeadca908be48e0 Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Wed, 20 Mar 2024 12:09:45 -0600 Subject: [PATCH 08/21] gui updates initializing setspinboxes --- rosplane_tuning/gui/tuning_functionality.py | 8 ++++---- rosplane_tuning/gui/tuning_gui.py | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 16d7aef..bd8b97c 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -129,15 +129,15 @@ def set_sliders(self): def set_SpinBoxes(self): # Sliders have an integer range. Set this from +- 100 - self.kpSpinBox.setValue(0) + self.kpSpinBox.setValue(self.curr_kp) self.kpSpinBox.setMinimum(-100) self.kpSpinBox.setMaximum(100) - self.kiSpinBox.setValue(0) + self.kiSpinBox.setValue(self.curr_ki) self.kiSpinBox.setMinimum(-100) self.kiSpinBox.setMaximum(100) - self.kdSpinBox.setValue(0) + self.kdSpinBox.setValue(self.curr_kd) self.kdSpinBox.setMinimum(-100) self.kdSpinBox.setMaximum(100) @@ -165,7 +165,7 @@ def kpSpinBox_callback(self): kpSpinBox_value = self.kpSpinBox.value() self.temp_kp = kpSpinBox_value slider_val = self.temp_kp*100/self.kp_edit_dist - self.kpSlider.setValue(slider_val) + self.kpSlider.setValue(int(slider_val)) def kiSpinBox_callback(self): kiSpinBox_value = self.kiSpinBox.value() diff --git a/rosplane_tuning/gui/tuning_gui.py b/rosplane_tuning/gui/tuning_gui.py index 0719256..6d235c8 100644 --- a/rosplane_tuning/gui/tuning_gui.py +++ b/rosplane_tuning/gui/tuning_gui.py @@ -120,9 +120,9 @@ def setupUi(self, MainWindow): self.kiSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) self.kiSpinBox.setObjectName("kiSpinBox") self.verticalLayout_5.addWidget(self.kiSpinBox) - self.kdSpinBox_2 = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) - self.kdSpinBox_2.setObjectName("kdSpinBox_2") - self.verticalLayout_5.addWidget(self.kdSpinBox_2) + self.kdSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) + self.kdSpinBox.setObjectName("kdSpinBox") + self.verticalLayout_5.addWidget(self.kdSpinBox) self.horizontalLayout.addLayout(self.verticalLayout_5) MainWindow.setCentralWidget(self.centralwidget) From c595141c580868cf31647863b0d51262a4c21db7 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Wed, 20 Mar 2024 14:45:14 -0600 Subject: [PATCH 09/21] Bug fix in the slider / spinbox update sequences --- rosplane_tuning/gui/tuning_functionality.py | 167 +++++++++++--------- rosplane_tuning/gui/tuning_gui.py | 9 ++ 2 files changed, 97 insertions(+), 79 deletions(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index bd8b97c..188b608 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -7,9 +7,15 @@ import subprocess import timeit +# TODO: +# 1. Add the clear button +# 2. Add the undo button +# 3. Make the GUI prettier (expanding frames, etc.) +# 4. Why is it so slow sometimes? + class Window(QMainWindow, Ui_MainWindow): - def __init__(self): #, parent: QWidget | None = ..., flags: Qt.WindowFlags | Qt.WindowType = ...) -> None: - super().__init__() #parent, flags) + def __init__(self): + super().__init__() self.setupUi(self) self.connectSignalSlots() @@ -40,65 +46,73 @@ def connectSignalSlots(self): self.airspeedButton.toggled.connect(self.airspeedButtonCallback) self.altitudeButton.toggled.connect(self.altitudeButtonCallback) self.runButton.clicked.connect(self.runButtonCallback) - self.kpSlider.sliderReleased.connect(self.kp_slider_callback) - self.kiSlider.sliderReleased.connect(self.ki_slider_callback) - self.kdSlider.sliderReleased.connect(self.kd_slider_callback) + self.kpSlider.valueChanged.connect(self.kp_slider_callback) + self.kiSlider.valueChanged.connect(self.ki_slider_callback) + self.kdSlider.valueChanged.connect(self.kd_slider_callback) + self.kpSpinBox.valueChanged.connect(self.kpSpinBox_callback) + self.kiSpinBox.valueChanged.connect(self.kiSpinBox_callback) + self.kdSpinBox.valueChanged.connect(self.kdSpinBox_callback) def courseButtonCallback(self): - # Set the tuning mode - self.tuning_mode = 'c' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - # Set the sliders to the appropriate values - self.set_sliders() - self.set_SpinBoxes() - self.initialize_temps() + if self.CourseButton.isChecked(): + # Set the tuning mode + self.tuning_mode = 'c' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + # Set the sliders to the appropriate values + self.set_sliders() + self.set_SpinBoxes() + self.initialize_temps() def rollButtonCallback(self): - # Set the tuning mode - self.tuning_mode = 'r' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - self.set_sliders() - self.set_SpinBoxes() - self.initialize_temps() + if self.rollButton.isChecked(): + # Set the tuning mode + self.tuning_mode = 'r' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + self.set_sliders() + self.set_SpinBoxes() + self.initialize_temps() def pitchButtonCallback(self): - # Set the tuning mode - self.tuning_mode = 'p' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - self.set_sliders() - self.set_SpinBoxes() - self.initialize_temps() + if self.pitchButton.isChecked(): + # Set the tuning mode + self.tuning_mode = 'p' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + self.set_sliders() + self.set_SpinBoxes() + self.initialize_temps() def airspeedButtonCallback(self): - # Set the tuning mode - self.tuning_mode = 'a_t' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - self.set_sliders() - self.set_SpinBoxes() - self.initialize_temps() + if self.airspeedButton.isChecked(): + # Set the tuning mode + self.tuning_mode = 'a_t' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + self.set_sliders() + self.set_SpinBoxes() + self.initialize_temps() def altitudeButtonCallback(self): - # Set the tuning mode - self.tuning_mode = 'a' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - self.set_sliders() - self.set_SpinBoxes() - self.initialize_temps() + if self.altitudeButton.isChecked(): + # Set the tuning mode + self.tuning_mode = 'a' + # Get the other parameters from ROS + self.curr_kp = self.get_param_output('p') + self.curr_kd = self.get_param_output('d') + self.curr_ki = self.get_param_output('i') + self.set_sliders() + self.set_SpinBoxes() + self.initialize_temps() def get_param_output(self, param:str) -> float: if self.time: start = timeit.timeit() @@ -115,32 +129,9 @@ def get_param_output(self, param:str) -> float: def set_sliders(self): # Sliders have an integer range. Set this from +- 100 self.kpSlider.setValue(0) - self.kpSlider.setMinimum(-100) - self.kpSlider.setMaximum(100) - self.kiSlider.setValue(0) - self.kiSlider.setMinimum(-100) - self.kiSlider.setMaximum(100) - self.kdSlider.setValue(0) - self.kdSlider.setMinimum(-100) - self.kdSlider.setMaximum(100) - - def set_SpinBoxes(self): - # Sliders have an integer range. Set this from +- 100 - self.kpSpinBox.setValue(self.curr_kp) - self.kpSpinBox.setMinimum(-100) - self.kpSpinBox.setMaximum(100) - - self.kiSpinBox.setValue(self.curr_ki) - self.kiSpinBox.setMinimum(-100) - self.kiSpinBox.setMaximum(100) - - self.kdSpinBox.setValue(self.curr_kd) - self.kdSpinBox.setMinimum(-100) - self.kdSpinBox.setMaximum(100) - def kp_slider_callback(self): slider_val = self.kpSlider.value() self.temp_kp = self.curr_kp + self.kp_edit_dist * slider_val / 100 @@ -160,24 +151,37 @@ def kd_slider_callback(self): self.kdSpinBox.setValue(self.temp_kd) + def set_SpinBoxes(self): + # Sliders have an integer range. Set this from +- 100 + self.kpSpinBox.setMinimum(self.curr_kp - self.kp_edit_dist) + self.kpSpinBox.setMaximum(self.curr_kp + self.kp_edit_dist) + self.kpSpinBox.setValue(self.curr_kp) + + self.kiSpinBox.setMinimum(self.curr_ki - self.ki_edit_dist) + self.kiSpinBox.setMaximum(self.curr_ki + self.ki_edit_dist) + self.kiSpinBox.setValue(self.curr_ki) + + self.kdSpinBox.setMinimum(self.curr_kd - self.kd_edit_dist) + self.kdSpinBox.setMaximum(self.curr_kd + self.kd_edit_dist) + self.kdSpinBox.setValue(self.curr_kd) def kpSpinBox_callback(self): kpSpinBox_value = self.kpSpinBox.value() self.temp_kp = kpSpinBox_value - slider_val = self.temp_kp*100/self.kp_edit_dist + slider_val = (self.temp_kp - self.curr_kp)*100/self.kp_edit_dist self.kpSlider.setValue(int(slider_val)) def kiSpinBox_callback(self): kiSpinBox_value = self.kiSpinBox.value() self.temp_ki = kiSpinBox_value - slider_val = self.temp_ki*100/self.ki_edit_dist - self.kiSlider.setValue(slider_val) + slider_val = (self.temp_ki - self.curr_ki)*100/self.ki_edit_dist + self.kiSlider.setValue(int(slider_val)) def kdSpinBox_callback(self): kdSpinBox_value = self.kdSpinBox.value() self.temp_kd = kdSpinBox_value - slider_val = self.temp_kd*100/self.kd_edit_dist - self.kdSlider.setValue(slider_val) + slider_val = (self.temp_kd - self.curr_kd)*100/self.kd_edit_dist + self.kdSlider.setValue(int(slider_val)) #slider stuff @@ -202,6 +206,11 @@ def runButtonCallback(self): print('Kp set to:', self.curr_kp) print('Ki set to:', self.curr_ki) print('Kd set to:', self.curr_kd) + + # Reinitialize the gui + self.set_sliders() + self.set_SpinBoxes() + self.initialize_temps() # Main loop diff --git a/rosplane_tuning/gui/tuning_gui.py b/rosplane_tuning/gui/tuning_gui.py index 6d235c8..bcde3d1 100644 --- a/rosplane_tuning/gui/tuning_gui.py +++ b/rosplane_tuning/gui/tuning_gui.py @@ -89,6 +89,8 @@ def setupUi(self, MainWindow): self.kpSlider.setMaximumSize(QtCore.QSize(400, 50)) self.kpSlider.setOrientation(QtCore.Qt.Horizontal) self.kpSlider.setObjectName("kpSlider") + self.kpSlider.setMinimum(-100) + self.kpSlider.setMaximum(100) self.verticalLayout_3.addWidget(self.kpSlider) self.kiSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) @@ -99,6 +101,8 @@ def setupUi(self, MainWindow): self.kiSlider.setMaximumSize(QtCore.QSize(400, 50)) self.kiSlider.setOrientation(QtCore.Qt.Horizontal) self.kiSlider.setObjectName("kiSlider") + self.kiSlider.setMinimum(-100) + self.kiSlider.setMaximum(100) self.verticalLayout_3.addWidget(self.kiSlider) self.kdSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) @@ -109,6 +113,8 @@ def setupUi(self, MainWindow): self.kdSlider.setMaximumSize(QtCore.QSize(400, 50)) self.kdSlider.setOrientation(QtCore.Qt.Horizontal) self.kdSlider.setObjectName("kdSlider") + self.kdSlider.setMinimum(-100) + self.kdSlider.setMaximum(100) self.verticalLayout_3.addWidget(self.kdSlider) self.horizontalLayout_3.addLayout(self.verticalLayout_3) self.horizontalLayout.addLayout(self.horizontalLayout_3) @@ -116,12 +122,15 @@ def setupUi(self, MainWindow): self.verticalLayout_5.setObjectName("verticalLayout_5") self.kpSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) self.kpSpinBox.setObjectName("kpSpinBox") + self.kpSpinBox.setSingleStep(0.01) self.verticalLayout_5.addWidget(self.kpSpinBox) self.kiSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) self.kiSpinBox.setObjectName("kiSpinBox") + self.kiSpinBox.setSingleStep(0.01) self.verticalLayout_5.addWidget(self.kiSpinBox) self.kdSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) self.kdSpinBox.setObjectName("kdSpinBox") + self.kdSpinBox.setSingleStep(0.01) self.verticalLayout_5.addWidget(self.kdSpinBox) self.horizontalLayout.addLayout(self.verticalLayout_5) MainWindow.setCentralWidget(self.centralwidget) From 79056008a71117df554069f74fe5943ef76763f8 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Wed, 20 Mar 2024 15:02:25 -0600 Subject: [PATCH 10/21] started adding undo button functionality --- rosplane_tuning/gui/tuning_functionality.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 188b608..9a80c2f 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -10,6 +10,7 @@ # TODO: # 1. Add the clear button # 2. Add the undo button +# 3. Add the save button # 3. Make the GUI prettier (expanding frames, etc.) # 4. Why is it so slow sometimes? @@ -25,6 +26,7 @@ def __init__(self): self.curr_kd = 0.0 self.curr_ki = 0.0 self.initialize_temps() + self.initialize_undos() # This allows us to have different ranges for fine tuning kp, ki, and kd self.kp_edit_dist = 2.0 self.ki_edit_dist = 0.5 @@ -38,6 +40,11 @@ def initialize_temps(self): self.temp_kd = 0.0 self.temp_ki = 0.0 + def initialize_undos(self): + self.undo_kp = self.curr_kp + self.undo_kd = self.curr_kd + self.undo_ki = self.curr_ki + def connectSignalSlots(self): # This is where we define signal slots (callbacks) for when the buttons get clicked self.CourseButton.toggled.connect(self.courseButtonCallback) @@ -64,7 +71,6 @@ def courseButtonCallback(self): # Set the sliders to the appropriate values self.set_sliders() self.set_SpinBoxes() - self.initialize_temps() def rollButtonCallback(self): if self.rollButton.isChecked(): @@ -76,7 +82,6 @@ def rollButtonCallback(self): self.curr_ki = self.get_param_output('i') self.set_sliders() self.set_SpinBoxes() - self.initialize_temps() def pitchButtonCallback(self): if self.pitchButton.isChecked(): @@ -88,7 +93,6 @@ def pitchButtonCallback(self): self.curr_ki = self.get_param_output('i') self.set_sliders() self.set_SpinBoxes() - self.initialize_temps() def airspeedButtonCallback(self): if self.airspeedButton.isChecked(): @@ -100,7 +104,6 @@ def airspeedButtonCallback(self): self.curr_ki = self.get_param_output('i') self.set_sliders() self.set_SpinBoxes() - self.initialize_temps() def altitudeButtonCallback(self): if self.altitudeButton.isChecked(): @@ -112,7 +115,6 @@ def altitudeButtonCallback(self): self.curr_ki = self.get_param_output('i') self.set_sliders() self.set_SpinBoxes() - self.initialize_temps() def get_param_output(self, param:str) -> float: if self.time: start = timeit.timeit() @@ -190,6 +192,10 @@ def kdSpinBox_callback(self): def runButtonCallback(self): #call this if run button is pushed + # Set the undo values to be the current values + self.undo_kp = self.curr_kp + self.undo_ki = self.curr_ki + self.undo_kd = self.curr_kd # Set current variables to be temp variables self.curr_kp = self.temp_kp self.curr_ki = self.temp_ki @@ -210,7 +216,6 @@ def runButtonCallback(self): # Reinitialize the gui self.set_sliders() self.set_SpinBoxes() - self.initialize_temps() # Main loop From 2c65517f439a2f616abc2b4fca5d4218925be06c Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Wed, 20 Mar 2024 16:35:04 -0600 Subject: [PATCH 11/21] gui updates setting up clear and save buttons --- rosplane_tuning/gui/tuning_functionality.py | 67 +++++++++++++++++++-- 1 file changed, 63 insertions(+), 4 deletions(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 9a80c2f..7481f1f 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -19,7 +19,25 @@ def __init__(self): super().__init__() self.setupUi(self) self.connectSignalSlots() + self.call_originals() + # Original parameters saved at init, called with clear button + self.orig_c_kp = 0 #original course parameters + self.orig_c_ki = 0 + self.orig_c_kd = 0 + self.orig_p_kp = 0 #original pitch parameters + self.orig_p_ki = 0 + self.orig_p_kd = 0 + self.orig_r_kp = 0 #original roll parameters + self.orig_r_ki = 0 + self.orig_r_kd = 0 + self.orig_a_t_kp = 0 #original airspeed (throttle) parameters + self.orig_a_t_ki = 0 + self.orig_a_t_kd = 0 + self.orig_a_kp = 0 #original altitude parameters + self.orig_a_ki = 0 + self.orig_a_kd = 0 + # Object Attributes that we use, with initializations self.tuning_mode = '' self.curr_kp = 0.0 @@ -35,6 +53,20 @@ def __init__(self): self.time = False self.disp = True + def call_originals(self): + modes = ["c","p","r","a_t","a"] + params = ['p','i','d'] + for mode in modes: + for param in params: + output = subprocess.run(["ros2","param","get","/autopilot",f"{mode}_k{param}"], stdout=subprocess.PIPE) + output_list = output.stdout.split() + output_val = output_list[-1] + + # Dynamically generate variable names and assign values + var_name = f"orig_{mode}_k{param}" + setattr(self, var_name, output_val) + + def initialize_temps(self): self.temp_kp = 0.0 self.temp_kd = 0.0 @@ -53,6 +85,8 @@ def connectSignalSlots(self): self.airspeedButton.toggled.connect(self.airspeedButtonCallback) self.altitudeButton.toggled.connect(self.altitudeButtonCallback) self.runButton.clicked.connect(self.runButtonCallback) + self.clearButton.clicked.connect(self.clearButtonCallback) + self.saveButton.clicked.connect(self.saveButtonCallback) self.kpSlider.valueChanged.connect(self.kp_slider_callback) self.kiSlider.valueChanged.connect(self.ki_slider_callback) self.kdSlider.valueChanged.connect(self.kd_slider_callback) @@ -186,10 +220,6 @@ def kdSpinBox_callback(self): self.kdSlider.setValue(int(slider_val)) - #slider stuff - #self.slider.valueChanged.connect(self.slider_callback) - #put somewhere else "self.runButton.clicked.connect(self.runButtonCallback)"" - def runButtonCallback(self): #call this if run button is pushed # Set the undo values to be the current values @@ -218,6 +248,35 @@ def runButtonCallback(self): self.set_SpinBoxes() + def clearButtonCallback(self): + params = ['p','i','d'] + for param in params: + orig_var_name = f"orig_{self.tuning_mode}_k{param}" + #get parameter values for orig_var_name + original_value = getattr(orig_var_name) + #generate curr param variable names + curr_var_name = f"curr_k{param}" + #Assign original values to curr parameters + setattr(self, curr_var_name, original_value) + #run button callback to apply changes + self.runButtonCallback() + + def saveButtonCallback(self): + modes = ["c","p","r","a_t","a"] + params = ['p','i','d'] + for mode in modes: + for param in params: + output = subprocess.run(["ros2","param","get","/autopilot",f"{mode}_k{param}"], stdout=subprocess.PIPE) + output_list = output.stdout.split() + output_val = output_list[-1] + + # Dynamically generate variable names and assign values + var_name = f"orig_{mode}_k{param}" + setattr(self, var_name, output_val) + self.runButtonCallback() + + + # Main loop if __name__=='__main__': app = QApplication(sys.argv) From 1d3a30a66ff99abcdeabc1dc90c521bc71666b49 Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Wed, 20 Mar 2024 16:38:48 -0600 Subject: [PATCH 12/21] changed clear button name --- rosplane_tuning/gui/tuning_gui.py | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/rosplane_tuning/gui/tuning_gui.py b/rosplane_tuning/gui/tuning_gui.py index bcde3d1..a65056b 100644 --- a/rosplane_tuning/gui/tuning_gui.py +++ b/rosplane_tuning/gui/tuning_gui.py @@ -58,9 +58,12 @@ def setupUi(self, MainWindow): self.runButton = QtWidgets.QPushButton(self.horizontalLayoutWidget) self.runButton.setObjectName("runButton") self.verticalLayout.addWidget(self.runButton) - self.Clear = QtWidgets.QPushButton(self.horizontalLayoutWidget) - self.Clear.setObjectName("Clear") - self.verticalLayout.addWidget(self.Clear) + self.saveButton = QtWidgets.QPushButton(self.horizontalLayoutWidget) + self.saveButton.setObjectName("saveButton") + self.verticalLayout.addWidget(self.saveButton) + self.clearButton = QtWidgets.QPushButton(self.horizontalLayoutWidget) + self.clearButton.setObjectName("Clear") + self.verticalLayout.addWidget(self.clearButton) self.horizontalLayout.addLayout(self.verticalLayout) self.horizontalLayout_3 = QtWidgets.QHBoxLayout() self.horizontalLayout_3.setObjectName("horizontalLayout_3") @@ -89,8 +92,6 @@ def setupUi(self, MainWindow): self.kpSlider.setMaximumSize(QtCore.QSize(400, 50)) self.kpSlider.setOrientation(QtCore.Qt.Horizontal) self.kpSlider.setObjectName("kpSlider") - self.kpSlider.setMinimum(-100) - self.kpSlider.setMaximum(100) self.verticalLayout_3.addWidget(self.kpSlider) self.kiSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) @@ -101,8 +102,6 @@ def setupUi(self, MainWindow): self.kiSlider.setMaximumSize(QtCore.QSize(400, 50)) self.kiSlider.setOrientation(QtCore.Qt.Horizontal) self.kiSlider.setObjectName("kiSlider") - self.kiSlider.setMinimum(-100) - self.kiSlider.setMaximum(100) self.verticalLayout_3.addWidget(self.kiSlider) self.kdSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) @@ -113,8 +112,6 @@ def setupUi(self, MainWindow): self.kdSlider.setMaximumSize(QtCore.QSize(400, 50)) self.kdSlider.setOrientation(QtCore.Qt.Horizontal) self.kdSlider.setObjectName("kdSlider") - self.kdSlider.setMinimum(-100) - self.kdSlider.setMaximum(100) self.verticalLayout_3.addWidget(self.kdSlider) self.horizontalLayout_3.addLayout(self.verticalLayout_3) self.horizontalLayout.addLayout(self.horizontalLayout_3) @@ -122,15 +119,12 @@ def setupUi(self, MainWindow): self.verticalLayout_5.setObjectName("verticalLayout_5") self.kpSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) self.kpSpinBox.setObjectName("kpSpinBox") - self.kpSpinBox.setSingleStep(0.01) self.verticalLayout_5.addWidget(self.kpSpinBox) self.kiSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) self.kiSpinBox.setObjectName("kiSpinBox") - self.kiSpinBox.setSingleStep(0.01) self.verticalLayout_5.addWidget(self.kiSpinBox) self.kdSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) self.kdSpinBox.setObjectName("kdSpinBox") - self.kdSpinBox.setSingleStep(0.01) self.verticalLayout_5.addWidget(self.kdSpinBox) self.horizontalLayout.addLayout(self.verticalLayout_5) MainWindow.setCentralWidget(self.centralwidget) From ac52b470e19c48517e4accbc9a2e725b0011273e Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Wed, 20 Mar 2024 16:45:12 -0600 Subject: [PATCH 13/21] changed clear button name --- rosplane_tuning/gui/tuning_gui.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosplane_tuning/gui/tuning_gui.py b/rosplane_tuning/gui/tuning_gui.py index a65056b..c6f6f9a 100644 --- a/rosplane_tuning/gui/tuning_gui.py +++ b/rosplane_tuning/gui/tuning_gui.py @@ -141,7 +141,7 @@ def retranslateUi(self, MainWindow): self.airspeedButton.setText(_translate("MainWindow", "Airspeed")) self.altitudeButton.setText(_translate("MainWindow", "Altitude")) self.runButton.setText(_translate("MainWindow", "Run")) - self.Clear.setText(_translate("MainWindow", "Clear")) + self.clearButton.setText(_translate("MainWindow", "Clear")) self.label_2.setText(_translate("MainWindow", "k_p")) self.label.setText(_translate("MainWindow", "k_i")) self.label_3.setText(_translate("MainWindow", "k_d")) From 441b1532a0810956d45def6f685ebb5bd71044c1 Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Wed, 20 Mar 2024 16:46:36 -0600 Subject: [PATCH 14/21] changed save button name --- rosplane_tuning/gui/tuning_gui.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rosplane_tuning/gui/tuning_gui.py b/rosplane_tuning/gui/tuning_gui.py index c6f6f9a..b907c51 100644 --- a/rosplane_tuning/gui/tuning_gui.py +++ b/rosplane_tuning/gui/tuning_gui.py @@ -142,6 +142,7 @@ def retranslateUi(self, MainWindow): self.altitudeButton.setText(_translate("MainWindow", "Altitude")) self.runButton.setText(_translate("MainWindow", "Run")) self.clearButton.setText(_translate("MainWindow", "Clear")) + self.saveButton.setText(_translate("MainWindow", "Save")) self.label_2.setText(_translate("MainWindow", "k_p")) self.label.setText(_translate("MainWindow", "k_i")) self.label_3.setText(_translate("MainWindow", "k_d")) From 0c916871d9d06d63248a151b01691cc148d348df Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Wed, 20 Mar 2024 17:56:32 -0600 Subject: [PATCH 15/21] draft versions of clear and save button, still work to be done --- rosplane_tuning/gui/tuning_functionality.py | 27 ++++++++++++--------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 7481f1f..254861f 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -61,10 +61,10 @@ def call_originals(self): output = subprocess.run(["ros2","param","get","/autopilot",f"{mode}_k{param}"], stdout=subprocess.PIPE) output_list = output.stdout.split() output_val = output_list[-1] - # Dynamically generate variable names and assign values var_name = f"orig_{mode}_k{param}" setattr(self, var_name, output_val) + print(f'{var_name} set to',output_val) def initialize_temps(self): @@ -253,26 +253,31 @@ def clearButtonCallback(self): for param in params: orig_var_name = f"orig_{self.tuning_mode}_k{param}" #get parameter values for orig_var_name - original_value = getattr(orig_var_name) + original_value = getattr(self,orig_var_name) #generate curr param variable names curr_var_name = f"curr_k{param}" #Assign original values to curr parameters setattr(self, curr_var_name, original_value) + print(f'{curr_var_name} set to {original_value}') #run button callback to apply changes self.runButtonCallback() def saveButtonCallback(self): - modes = ["c","p","r","a_t","a"] - params = ['p','i','d'] + modes = ["c", "p", "r", "a_t", "a"] + params = ['p', 'i', 'd'] for mode in modes: for param in params: - output = subprocess.run(["ros2","param","get","/autopilot",f"{mode}_k{param}"], stdout=subprocess.PIPE) - output_list = output.stdout.split() - output_val = output_list[-1] - - # Dynamically generate variable names and assign values - var_name = f"orig_{mode}_k{param}" - setattr(self, var_name, output_val) + try: + #Rn this only calls the original param values and renames them + output = subprocess.run(["ros2", "param", "get", "/autopilot", f"{mode}_k{param}"], stdout=subprocess.PIPE, check=True) + output_list = output.stdout.split() + output_val = output_list[-1] + # Dynamically generate variable names and assign values + var_name = f"orig_{mode}_k{param}" + setattr(self, var_name, output_val) + print(f'{var_name} set to {output_val}') + except subprocess.CalledProcessError as e: + print(f"Failed to get parameter value for {mode}_k{param}: {e}") self.runButtonCallback() From ef33a7fac72fec087202f97e25de9b6712ec3eee Mon Sep 17 00:00:00 2001 From: Joseph Ritchie Date: Thu, 21 Mar 2024 10:07:24 -0600 Subject: [PATCH 16/21] working through the save and clear buttons --- rosplane_tuning/gui/tuning_functionality.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 254861f..ff9ff91 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -19,7 +19,6 @@ def __init__(self): super().__init__() self.setupUi(self) self.connectSignalSlots() - self.call_originals() # Original parameters saved at init, called with clear button self.orig_c_kp = 0 #original course parameters @@ -38,6 +37,8 @@ def __init__(self): self.orig_a_ki = 0 self.orig_a_kd = 0 + self.call_originals() + # Object Attributes that we use, with initializations self.tuning_mode = '' self.curr_kp = 0.0 @@ -248,14 +249,14 @@ def runButtonCallback(self): self.set_SpinBoxes() - def clearButtonCallback(self): + def clearButtonCallback(self): #resets the current mode's inputs to original or last save values params = ['p','i','d'] for param in params: orig_var_name = f"orig_{self.tuning_mode}_k{param}" #get parameter values for orig_var_name original_value = getattr(self,orig_var_name) #generate curr param variable names - curr_var_name = f"curr_k{param}" + curr_var_name = f"temp_k{param}" #Assign original values to curr parameters setattr(self, curr_var_name, original_value) print(f'{curr_var_name} set to {original_value}') @@ -278,7 +279,7 @@ def saveButtonCallback(self): print(f'{var_name} set to {output_val}') except subprocess.CalledProcessError as e: print(f"Failed to get parameter value for {mode}_k{param}: {e}") - self.runButtonCallback() + From b647fdd9ad76fee9bc33fa5fba74c027590fd624 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Thu, 21 Mar 2024 10:51:48 -0600 Subject: [PATCH 17/21] bug fix in clear button, set size of sliders in init function --- rosplane_tuning/gui/tuning_functionality.py | 28 +++++++++++++++------ 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index ff9ff91..42d3295 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -19,6 +19,7 @@ def __init__(self): super().__init__() self.setupUi(self) self.connectSignalSlots() + self.set_sizes() # Original parameters saved at init, called with clear button self.orig_c_kp = 0 #original course parameters @@ -54,6 +55,14 @@ def __init__(self): self.time = False self.disp = True + def set_sizes(self): + self.kpSlider.setMinimum(-100) + self.kpSlider.setMaximum(100) + self.kiSlider.setMinimum(-100) + self.kiSlider.setMaximum(100) + self.kdSlider.setMinimum(-100) + self.kdSlider.setMaximum(100) + def call_originals(self): modes = ["c","p","r","a_t","a"] params = ['p','i','d'] @@ -96,6 +105,7 @@ def connectSignalSlots(self): self.kdSpinBox.valueChanged.connect(self.kdSpinBox_callback) def courseButtonCallback(self): + if self.disp: print("COURSE gains selected") if self.CourseButton.isChecked(): # Set the tuning mode self.tuning_mode = 'c' @@ -108,6 +118,7 @@ def courseButtonCallback(self): self.set_SpinBoxes() def rollButtonCallback(self): + if self.disp: print("ROLL gains selected") if self.rollButton.isChecked(): # Set the tuning mode self.tuning_mode = 'r' @@ -119,6 +130,7 @@ def rollButtonCallback(self): self.set_SpinBoxes() def pitchButtonCallback(self): + if self.disp: print("PITCH gains selected") if self.pitchButton.isChecked(): # Set the tuning mode self.tuning_mode = 'p' @@ -130,6 +142,7 @@ def pitchButtonCallback(self): self.set_SpinBoxes() def airspeedButtonCallback(self): + if self.disp: print("AIRSPEED gains selected") if self.airspeedButton.isChecked(): # Set the tuning mode self.tuning_mode = 'a_t' @@ -141,6 +154,7 @@ def airspeedButtonCallback(self): self.set_SpinBoxes() def altitudeButtonCallback(self): + if self.disp: print("ALTITUDE gains selected") if self.altitudeButton.isChecked(): # Set the tuning mode self.tuning_mode = 'a' @@ -172,19 +186,19 @@ def set_sliders(self): def kp_slider_callback(self): slider_val = self.kpSlider.value() self.temp_kp = self.curr_kp + self.kp_edit_dist * slider_val / 100 - if self.disp: print(self.temp_kp) + # if self.disp: print(self.temp_kp) self.kpSpinBox.setValue(self.temp_kp) def ki_slider_callback(self): slider_val = self.kiSlider.value() self.temp_ki = self.curr_ki + self.ki_edit_dist * slider_val / 100 - if self.disp: print(self.temp_ki) + # if self.disp: print(self.temp_ki) self.kiSpinBox.setValue(self.temp_ki) def kd_slider_callback(self): slider_val = self.kdSlider.value() self.temp_kd = self.curr_kd + self.kd_edit_dist * slider_val / 100 - if self.disp: print(slider_val, self.temp_kd) + # if self.disp: print(self.temp_kd) self.kdSpinBox.setValue(self.temp_kd) @@ -222,6 +236,7 @@ def kdSpinBox_callback(self): def runButtonCallback(self): + if self.disp: print('RUNNING parameters') #call this if run button is pushed # Set the undo values to be the current values self.undo_kp = self.curr_kp @@ -250,6 +265,7 @@ def runButtonCallback(self): def clearButtonCallback(self): #resets the current mode's inputs to original or last save values + if self.disp: print('\nCLEARING <' + self.tuning_mode + '> parameters') params = ['p','i','d'] for param in params: orig_var_name = f"orig_{self.tuning_mode}_k{param}" @@ -258,12 +274,13 @@ def clearButtonCallback(self): #resets the current mode's inputs to origina #generate curr param variable names curr_var_name = f"temp_k{param}" #Assign original values to curr parameters - setattr(self, curr_var_name, original_value) + setattr(self, curr_var_name, float(original_value)) print(f'{curr_var_name} set to {original_value}') #run button callback to apply changes self.runButtonCallback() def saveButtonCallback(self): + if self.disp: print('\nSAVING all parameters') modes = ["c", "p", "r", "a_t", "a"] params = ['p', 'i', 'd'] for mode in modes: @@ -279,9 +296,6 @@ def saveButtonCallback(self): print(f'{var_name} set to {output_val}') except subprocess.CalledProcessError as e: print(f"Failed to get parameter value for {mode}_k{param}: {e}") - - - # Main loop if __name__=='__main__': From 0016d71a415cd837fbb39543076ba8b0e2a8322c Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Thu, 21 Mar 2024 11:18:41 -0600 Subject: [PATCH 18/21] add smaller step size to spin boxes --- rosplane_tuning/gui/tuning_functionality.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 42d3295..2adb326 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -62,6 +62,9 @@ def set_sizes(self): self.kiSlider.setMaximum(100) self.kdSlider.setMinimum(-100) self.kdSlider.setMaximum(100) + self.kpSpinBox.setSingleStep(0.01) + self.kiSpinBox.setSingleStep(0.01) + self.kdSpinBox.setSingleStep(0.01) def call_originals(self): modes = ["c","p","r","a_t","a"] From e44d499fd956e7e483966be1ee07e696374927c6 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Tue, 23 Apr 2024 17:32:55 -0600 Subject: [PATCH 19/21] converted gui to an rqt gui --- rosplane_tuning/CMakeLists.txt | 37 ++++++++++- rosplane_tuning/gui/tuning_functionality.py | 1 - rosplane_tuning/package.xml | 3 +- rosplane_tuning/plugin.xml | 17 +++++ .../{gui => resource}/tuning_gui.ui | 4 +- rosplane_tuning/scripts/rqt_tuning_gui | 9 +++ .../src/rqt_tuning_gui/__init__.py | 0 .../rqt_tuning_gui/rosflight_tuning_gui.py | 64 +++++++++++++++++++ 8 files changed, 130 insertions(+), 5 deletions(-) create mode 100644 rosplane_tuning/plugin.xml rename rosplane_tuning/{gui => resource}/tuning_gui.ui (98%) create mode 100644 rosplane_tuning/scripts/rqt_tuning_gui create mode 100644 rosplane_tuning/src/rqt_tuning_gui/__init__.py create mode 100644 rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py diff --git a/rosplane_tuning/CMakeLists.txt b/rosplane_tuning/CMakeLists.txt index 631836a..9174b8d 100644 --- a/rosplane_tuning/CMakeLists.txt +++ b/rosplane_tuning/CMakeLists.txt @@ -44,7 +44,7 @@ include_directories( #use this if you need .h files for include statements. Th install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) -### START OF REAL EXECUTABLES ### +### START OF EXECUTABLES ### # Signal Generator add_executable(signal_generator @@ -65,4 +65,39 @@ install(PROGRAMS #### END OF EXECUTABLES ### +#### RQT #### + +ament_python_install_package(rqt_tuning_gui + PACKAGE_DIR src/rqt_tuning_gui +) + +install(FILES + plugin.xml + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY + resource + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS + scripts/rqt_tuning_gui + DESTINATION lib/${PROJECT_NAME} +) + +#### END OF RQT #### + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + ament_package() diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py index 2adb326..2799e08 100644 --- a/rosplane_tuning/gui/tuning_functionality.py +++ b/rosplane_tuning/gui/tuning_functionality.py @@ -5,7 +5,6 @@ from PyQt5.uic import loadUi from tuning_gui import Ui_MainWindow import subprocess -import timeit # TODO: # 1. Add the clear button diff --git a/rosplane_tuning/package.xml b/rosplane_tuning/package.xml index 9323df2..0d1503a 100644 --- a/rosplane_tuning/package.xml +++ b/rosplane_tuning/package.xml @@ -3,7 +3,7 @@ rosplane_tuning 1.0.0 - TODO: Package description + Contains data visualization, signal generator, and a RQT-based tuning GUI. controls BSD @@ -23,6 +23,7 @@ ament_cmake + diff --git a/rosplane_tuning/plugin.xml b/rosplane_tuning/plugin.xml new file mode 100644 index 0000000..38dd565 --- /dev/null +++ b/rosplane_tuning/plugin.xml @@ -0,0 +1,17 @@ + + + + Tuning GUI for ROSflight + + + + + + + system-help + Super helpful GUI for tuning ROSplane. + + + diff --git a/rosplane_tuning/gui/tuning_gui.ui b/rosplane_tuning/resource/tuning_gui.ui similarity index 98% rename from rosplane_tuning/gui/tuning_gui.ui rename to rosplane_tuning/resource/tuning_gui.ui index 4128e02..cbfdea4 100644 --- a/rosplane_tuning/gui/tuning_gui.ui +++ b/rosplane_tuning/resource/tuning_gui.ui @@ -1,7 +1,7 @@ - MainWindow - +Form + 0 diff --git a/rosplane_tuning/scripts/rqt_tuning_gui b/rosplane_tuning/scripts/rqt_tuning_gui new file mode 100644 index 0000000..4987816 --- /dev/null +++ b/rosplane_tuning/scripts/rqt_tuning_gui @@ -0,0 +1,9 @@ +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages = ['rqt_tuning_gui'], + package_dir = {'': 'src'}, +) + +setup(**d) diff --git a/rosplane_tuning/src/rqt_tuning_gui/__init__.py b/rosplane_tuning/src/rqt_tuning_gui/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py b/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py new file mode 100644 index 0000000..590eeaa --- /dev/null +++ b/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py @@ -0,0 +1,64 @@ +import os +import rclpy + +from qt_gui.plugin import Plugin +from python_qt_binding import loadUi +from python_qt_binding.QtWidgets import QWidget + +from ament_index_python import get_resource + +class ROSflightGUI(Plugin): + def __init__(self, context): + super(ROSflightGUI, self).__init__(context) + # Give QObjects reasonable names + self.setObjectName('ROSflightGUI') + + # Process standalone plugin command-line arguments + from argparse import ArgumentParser + parser = ArgumentParser() + # Add argument(s) to the parser. + parser.add_argument("-q", "--quiet", action="store_true", + dest="quiet", + help="Put plugin in silent mode") + args, unknowns = parser.parse_known_args(context.argv()) + if not args.quiet: + print('arguments: ', args) + print('unknowns: ', unknowns) + + # Create QWidget + self._widget = QWidget() + # Get path to UI file which should be in the "resource" folder of this package + _, path = get_resource('packages', 'rosplane_tuning') + ui_file = os.path.join(path, 'share', 'rosplane_tuning', 'resource', 'tuning_gui.ui') + # Extend the widget with all attributes and children from UI file + loadUi(ui_file, self._widget) + # Give QObjects reasonable names + self._widget.setObjectName('ROSflightTuningUi') + # Show _widget.windowTitle on left-top of each plugin (when + # it's set in _widget). This is useful when you open multiple + # plugins at once. Also if you open multiple instances of your + # plugin at once, these lines add number to make it easy to + # tell from pane to pane. + if context.serial_number() > 1: + self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) + # Add widget to the user interface + context.add_widget(self._widget) + + def shutdown_plugin(self): + # TODO unregister all publishers here + pass + + def save_settings(self, plugin_settings, instance_settings): + # TODO save intrinsic configuration, usually using: + # instance_settings.set_value(k, v) + pass + + def restore_settings(self, plugin_settings, instance_settings): + # TODO restore intrinsic configuration, usually using: + # v = instance_settings.value(k) + pass + + #def trigger_configuration(self): + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget title bar + # Usually used to open a modal configuration dialog From 731b71fe4d4b57ce616fc1b19d95c8bde7236ea4 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Wed, 24 Apr 2024 13:35:00 -0600 Subject: [PATCH 20/21] got rqt to pull up our widget --- rosplane_tuning/resource/tuning_gui.ui | 548 +++++++++++++----- .../rqt_tuning_gui/rosflight_tuning_gui.py | 116 +++- 2 files changed, 523 insertions(+), 141 deletions(-) diff --git a/rosplane_tuning/resource/tuning_gui.ui b/rosplane_tuning/resource/tuning_gui.ui index cbfdea4..c707041 100644 --- a/rosplane_tuning/resource/tuning_gui.ui +++ b/rosplane_tuning/resource/tuning_gui.ui @@ -1,221 +1,489 @@ -Form - + ROSPlaneTuningGUI + 0 0 - 810 - 600 + 854 + 658 - + 0 0 + + + 810 + 610 + + MainWindow - + + + + 20 + 20 + 810 + 631 + + 0 0 - + + + 800 + 600 + + + + + 16777215 + 16777215 + + + - 20 + 10 10 - 761 - 571 + 791 + 611 - - - 6 - + - QLayout::SetFixedSize - - - 0 + QLayout::SetDefaultConstraint - + + + + 16777215 + 50 + + + + <html><head/><body><p align="center"></p><p align="center"><span style=" font-size:16pt;">ROSplane Tuning GUI</span></p></body></html> + + + + + 6 - QLayout::SetMaximumSize + QLayout::SetDefaultConstraint - 50 + 0 - - - Course - - - - - - - Roll - - - - - - - Pitch - - - - - - - Airspeed - - - - - - - Altitude + + + 6 - - - - - - Run + + QLayout::SetDefaultConstraint - - - - - - Clear + + 50 - - - - - - - - - + - k_p + Course - + - k_i + Roll - + - k_d + Pitch - - - - - - QLayout::SetDefaultConstraint - - - 0 - - - 0 - - - - - 0 - 0 - - - - - 400 - 50 - - - - Qt::Horizontal + + + Airspeed - - - - 0 - 0 - - - - - 400 - 50 - + + + Altitude - - Qt::Horizontal + + + + + + Run - + - + 0 0 - + - 400 - 50 + 0 + 0 - - Qt::Horizontal + + Clear - - - - - - - - - - - + + + QLayout::SetDefaultConstraint + + + 0 + + + 0 + + + + + + + + + + 70 + 16777215 + + + + <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">p </span><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 0 + + + + + + + + + + QFrame::Plain + + + 1 + + + Qt::Vertical + + + + + + + 10 + + + + + Previous Value: + + + + + + + Saved Value: + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 40 + + + + Qt::Horizontal + + + QSlider::TicksBelow + + + 0 + + + + + + + Qt::Vertical + + + QSizePolicy::Maximum + + + + 20 + 40 + + + + + + + + + + + + + + + 70 + 16777215 + + + + <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">i </span><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 0 + + + + + + + + + + QFrame::Plain + + + 1 + + + Qt::Vertical + + + + + + + 10 + + + + + Previous Value: + + + + + + + Saved Value: + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 40 + + + + Qt::Horizontal + + + QSlider::TicksBelow + + + + + + + Qt::Vertical + + + QSizePolicy::Maximum + + + + 20 + 40 + + + + + + + + + + + + + + + 70 + 16777215 + + + + <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">d</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;"/><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 0 + + + + + + + + + + QFrame::Plain + + + 1 + + + Qt::Vertical + + + + + + + 10 + + + + + Previous Value: + + + + + + + Saved Value: + + + + + + + + + + + + 0 + 0 + + + + + 16777215 + 40 + + + + Qt::Horizontal + + + QSlider::TicksBelow + + + + + + + Qt::Vertical + + + QSizePolicy::Maximum + + + + 20 + 40 + + + + + + + @@ -223,6 +491,8 @@ - + + + diff --git a/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py b/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py index 590eeaa..de11f57 100644 --- a/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py +++ b/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py @@ -3,7 +3,8 @@ from qt_gui.plugin import Plugin from python_qt_binding import loadUi -from python_qt_binding.QtWidgets import QWidget +from python_qt_binding.QtWidgets import QMainWindow, QWidget, QSizePolicy, QHBoxLayout, QVBoxLayout, QRadioButton, QPushButton, QLabel, QLayout, QSlider, QDoubleSpinBox +from python_qt_binding.QtCore import QSize, Qt, QRect from ament_index_python import get_resource @@ -26,7 +27,7 @@ def __init__(self, context): print('unknowns: ', unknowns) # Create QWidget - self._widget = QWidget() + self._widget = QMainWindow() # Get path to UI file which should be in the "resource" folder of this package _, path = get_resource('packages', 'rosplane_tuning') ui_file = os.path.join(path, 'share', 'rosplane_tuning', 'resource', 'tuning_gui.ui') @@ -44,6 +45,8 @@ def __init__(self, context): # Add widget to the user interface context.add_widget(self._widget) + # self.setupUi() + def shutdown_plugin(self): # TODO unregister all publishers here pass @@ -62,3 +65,112 @@ def restore_settings(self, plugin_settings, instance_settings): # Comment in to signal that the plugin has a way to configure # This will enable a setting button (gear icon) in each dock widget title bar # Usually used to open a modal configuration dialog + + def setupUi(self): + self.centralwidget = QWidget(self._widget) + sizePolicy = QSizePolicy(QSizePolicy.Preferred, QSizePolicy.Preferred) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.centralwidget.sizePolicy().hasHeightForWidth()) + self.centralwidget.setSizePolicy(sizePolicy) + self.centralwidget.setObjectName("centralwidget") + self.horizontalLayoutWidget = QWidget(self.centralwidget) + self.horizontalLayoutWidget.setGeometry(QRect(20, 10, 761, 571)) + self.horizontalLayoutWidget.setObjectName("horizontalLayoutWidget") + self.horizontalLayout = QHBoxLayout(self.horizontalLayoutWidget) + self.horizontalLayout.setSizeConstraint(QLayout.SetFixedSize) + self.horizontalLayout.setContentsMargins(0, 0, 0, 0) + self.horizontalLayout.setSpacing(6) + self.horizontalLayout.setObjectName("horizontalLayout") + self.verticalLayout = QVBoxLayout() + self.verticalLayout.setSizeConstraint(QLayout.SetMaximumSize) + self.verticalLayout.setContentsMargins(-1, -1, 50, -1) + self.verticalLayout.setSpacing(6) + self.verticalLayout.setObjectName("verticalLayout") + self.CourseButton = QRadioButton(self.horizontalLayoutWidget) + self.CourseButton.setObjectName("CourseButton") + self.verticalLayout.addWidget(self.CourseButton) + self.rollButton = QRadioButton(self.horizontalLayoutWidget) + self.rollButton.setObjectName("rollButton") + self.verticalLayout.addWidget(self.rollButton) + self.pitchButton = QRadioButton(self.horizontalLayoutWidget) + self.pitchButton.setObjectName("pitchButton") + self.verticalLayout.addWidget(self.pitchButton) + self.airspeedButton = QRadioButton(self.horizontalLayoutWidget) + self.airspeedButton.setObjectName("airspeedButton") + self.verticalLayout.addWidget(self.airspeedButton) + self.altitudeButton = QRadioButton(self.horizontalLayoutWidget) + self.altitudeButton.setObjectName("altitudeButton") + self.verticalLayout.addWidget(self.altitudeButton) + self.runButton = QPushButton(self.horizontalLayoutWidget) + self.runButton.setObjectName("runButton") + self.verticalLayout.addWidget(self.runButton) + self.saveButton = QPushButton(self.horizontalLayoutWidget) + self.saveButton.setObjectName("saveButton") + self.verticalLayout.addWidget(self.saveButton) + self.clearButton = QPushButton(self.horizontalLayoutWidget) + self.clearButton.setObjectName("Clear") + self.verticalLayout.addWidget(self.clearButton) + self.horizontalLayout.addLayout(self.verticalLayout) + self.horizontalLayout_3 = QHBoxLayout() + self.horizontalLayout_3.setObjectName("horizontalLayout_3") + self.verticalLayout_4 = QVBoxLayout() + self.verticalLayout_4.setObjectName("verticalLayout_4") + self.label_2 = QLabel(self.horizontalLayoutWidget) + self.label_2.setObjectName("label_2") + self.verticalLayout_4.addWidget(self.label_2) + self.label = QLabel(self.horizontalLayoutWidget) + self.label.setObjectName("label") + self.verticalLayout_4.addWidget(self.label) + self.label_3 = QLabel(self.horizontalLayoutWidget) + self.label_3.setObjectName("label_3") + self.verticalLayout_4.addWidget(self.label_3) + self.horizontalLayout_3.addLayout(self.verticalLayout_4) + self.verticalLayout_3 = QVBoxLayout() + self.verticalLayout_3.setSizeConstraint(QLayout.SetDefaultConstraint) + self.verticalLayout_3.setContentsMargins(0, -1, 0, -1) + self.verticalLayout_3.setObjectName("verticalLayout_3") + self.kpSlider = QSlider(self.horizontalLayoutWidget) + sizePolicy = QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.kpSlider.sizePolicy().hasHeightForWidth()) + self.kpSlider.setSizePolicy(sizePolicy) + self.kpSlider.setMaximumSize(QSize(400, 50)) + self.kpSlider.setOrientation(Qt.Horizontal) + self.kpSlider.setObjectName("kpSlider") + self.verticalLayout_3.addWidget(self.kpSlider) + self.kiSlider = QSlider(self.horizontalLayoutWidget) + sizePolicy = QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.kiSlider.sizePolicy().hasHeightForWidth()) + self.kiSlider.setSizePolicy(sizePolicy) + self.kiSlider.setMaximumSize(QSize(400, 50)) + self.kiSlider.setOrientation(Qt.Horizontal) + self.kiSlider.setObjectName("kiSlider") + self.verticalLayout_3.addWidget(self.kiSlider) + self.kdSlider = QSlider(self.horizontalLayoutWidget) + sizePolicy = QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed) + sizePolicy.setHorizontalStretch(0) + sizePolicy.setVerticalStretch(0) + sizePolicy.setHeightForWidth(self.kdSlider.sizePolicy().hasHeightForWidth()) + self.kdSlider.setSizePolicy(sizePolicy) + self.kdSlider.setMaximumSize(QSize(400, 50)) + self.kdSlider.setOrientation(Qt.Horizontal) + self.kdSlider.setObjectName("kdSlider") + self.verticalLayout_3.addWidget(self.kdSlider) + self.horizontalLayout_3.addLayout(self.verticalLayout_3) + self.horizontalLayout.addLayout(self.horizontalLayout_3) + self.verticalLayout_5 = QVBoxLayout() + self.verticalLayout_5.setObjectName("verticalLayout_5") + self.kpSpinBox = QDoubleSpinBox(self.horizontalLayoutWidget) + self.kpSpinBox.setObjectName("kpSpinBox") + self.verticalLayout_5.addWidget(self.kpSpinBox) + self.kiSpinBox = QDoubleSpinBox(self.horizontalLayoutWidget) + self.kiSpinBox.setObjectName("kiSpinBox") + self.verticalLayout_5.addWidget(self.kiSpinBox) + self.kdSpinBox = QDoubleSpinBox(self.horizontalLayoutWidget) + self.kdSpinBox.setObjectName("kdSpinBox") + self.verticalLayout_5.addWidget(self.kdSpinBox) + self.horizontalLayout.addLayout(self.verticalLayout_5) \ No newline at end of file From f0b92f0099d4993175b555c88a3994232f295856 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Mon, 29 Apr 2024 14:38:51 -0600 Subject: [PATCH 21/21] updated GUI, based on RQT --- rosplane_tuning/gui/tuning_functionality.py | 307 ----- rosplane_tuning/gui/tuning_gui.py | 148 --- rosplane_tuning/resource/tuning_gui.ui | 1007 +++++++++++------ .../rqt_tuning_gui/rosflight_tuning_gui.py | 575 ++++++++-- 4 files changed, 1154 insertions(+), 883 deletions(-) delete mode 100644 rosplane_tuning/gui/tuning_functionality.py delete mode 100644 rosplane_tuning/gui/tuning_gui.py diff --git a/rosplane_tuning/gui/tuning_functionality.py b/rosplane_tuning/gui/tuning_functionality.py deleted file mode 100644 index 2799e08..0000000 --- a/rosplane_tuning/gui/tuning_functionality.py +++ /dev/null @@ -1,307 +0,0 @@ -import sys -from PyQt5.QtCore import Qt -from PyQt5.QtWidgets import * -from PyQt5.QtWidgets import QWidget -from PyQt5.uic import loadUi -from tuning_gui import Ui_MainWindow -import subprocess - -# TODO: -# 1. Add the clear button -# 2. Add the undo button -# 3. Add the save button -# 3. Make the GUI prettier (expanding frames, etc.) -# 4. Why is it so slow sometimes? - -class Window(QMainWindow, Ui_MainWindow): - def __init__(self): - super().__init__() - self.setupUi(self) - self.connectSignalSlots() - self.set_sizes() - - # Original parameters saved at init, called with clear button - self.orig_c_kp = 0 #original course parameters - self.orig_c_ki = 0 - self.orig_c_kd = 0 - self.orig_p_kp = 0 #original pitch parameters - self.orig_p_ki = 0 - self.orig_p_kd = 0 - self.orig_r_kp = 0 #original roll parameters - self.orig_r_ki = 0 - self.orig_r_kd = 0 - self.orig_a_t_kp = 0 #original airspeed (throttle) parameters - self.orig_a_t_ki = 0 - self.orig_a_t_kd = 0 - self.orig_a_kp = 0 #original altitude parameters - self.orig_a_ki = 0 - self.orig_a_kd = 0 - - self.call_originals() - - # Object Attributes that we use, with initializations - self.tuning_mode = '' - self.curr_kp = 0.0 - self.curr_kd = 0.0 - self.curr_ki = 0.0 - self.initialize_temps() - self.initialize_undos() - # This allows us to have different ranges for fine tuning kp, ki, and kd - self.kp_edit_dist = 2.0 - self.ki_edit_dist = 0.5 - self.kd_edit_dist = 2.0 - # Boolean values for controlling debugging statements - self.time = False - self.disp = True - - def set_sizes(self): - self.kpSlider.setMinimum(-100) - self.kpSlider.setMaximum(100) - self.kiSlider.setMinimum(-100) - self.kiSlider.setMaximum(100) - self.kdSlider.setMinimum(-100) - self.kdSlider.setMaximum(100) - self.kpSpinBox.setSingleStep(0.01) - self.kiSpinBox.setSingleStep(0.01) - self.kdSpinBox.setSingleStep(0.01) - - def call_originals(self): - modes = ["c","p","r","a_t","a"] - params = ['p','i','d'] - for mode in modes: - for param in params: - output = subprocess.run(["ros2","param","get","/autopilot",f"{mode}_k{param}"], stdout=subprocess.PIPE) - output_list = output.stdout.split() - output_val = output_list[-1] - # Dynamically generate variable names and assign values - var_name = f"orig_{mode}_k{param}" - setattr(self, var_name, output_val) - print(f'{var_name} set to',output_val) - - - def initialize_temps(self): - self.temp_kp = 0.0 - self.temp_kd = 0.0 - self.temp_ki = 0.0 - - def initialize_undos(self): - self.undo_kp = self.curr_kp - self.undo_kd = self.curr_kd - self.undo_ki = self.curr_ki - - def connectSignalSlots(self): - # This is where we define signal slots (callbacks) for when the buttons get clicked - self.CourseButton.toggled.connect(self.courseButtonCallback) - self.pitchButton.toggled.connect(self.pitchButtonCallback) - self.rollButton.toggled.connect(self.rollButtonCallback) - self.airspeedButton.toggled.connect(self.airspeedButtonCallback) - self.altitudeButton.toggled.connect(self.altitudeButtonCallback) - self.runButton.clicked.connect(self.runButtonCallback) - self.clearButton.clicked.connect(self.clearButtonCallback) - self.saveButton.clicked.connect(self.saveButtonCallback) - self.kpSlider.valueChanged.connect(self.kp_slider_callback) - self.kiSlider.valueChanged.connect(self.ki_slider_callback) - self.kdSlider.valueChanged.connect(self.kd_slider_callback) - self.kpSpinBox.valueChanged.connect(self.kpSpinBox_callback) - self.kiSpinBox.valueChanged.connect(self.kiSpinBox_callback) - self.kdSpinBox.valueChanged.connect(self.kdSpinBox_callback) - - def courseButtonCallback(self): - if self.disp: print("COURSE gains selected") - if self.CourseButton.isChecked(): - # Set the tuning mode - self.tuning_mode = 'c' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - # Set the sliders to the appropriate values - self.set_sliders() - self.set_SpinBoxes() - - def rollButtonCallback(self): - if self.disp: print("ROLL gains selected") - if self.rollButton.isChecked(): - # Set the tuning mode - self.tuning_mode = 'r' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - self.set_sliders() - self.set_SpinBoxes() - - def pitchButtonCallback(self): - if self.disp: print("PITCH gains selected") - if self.pitchButton.isChecked(): - # Set the tuning mode - self.tuning_mode = 'p' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - self.set_sliders() - self.set_SpinBoxes() - - def airspeedButtonCallback(self): - if self.disp: print("AIRSPEED gains selected") - if self.airspeedButton.isChecked(): - # Set the tuning mode - self.tuning_mode = 'a_t' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - self.set_sliders() - self.set_SpinBoxes() - - def altitudeButtonCallback(self): - if self.disp: print("ALTITUDE gains selected") - if self.altitudeButton.isChecked(): - # Set the tuning mode - self.tuning_mode = 'a' - # Get the other parameters from ROS - self.curr_kp = self.get_param_output('p') - self.curr_kd = self.get_param_output('d') - self.curr_ki = self.get_param_output('i') - self.set_sliders() - self.set_SpinBoxes() - - def get_param_output(self, param:str) -> float: - if self.time: start = timeit.timeit() - output = subprocess.run(["ros2","param","get","/autopilot",f"{self.tuning_mode}_k{param}"], stdout=subprocess.PIPE) - try: - output_list = output.stdout.split() - output_val = output_list[-1] - if self.time: print(timeit.timeit() - start) - return float(output_val) - except IndexError: - print("Error accessing ROS service output! Output:", output.stdout) - return -1 - - def set_sliders(self): - # Sliders have an integer range. Set this from +- 100 - self.kpSlider.setValue(0) - self.kiSlider.setValue(0) - self.kdSlider.setValue(0) - - def kp_slider_callback(self): - slider_val = self.kpSlider.value() - self.temp_kp = self.curr_kp + self.kp_edit_dist * slider_val / 100 - # if self.disp: print(self.temp_kp) - self.kpSpinBox.setValue(self.temp_kp) - - def ki_slider_callback(self): - slider_val = self.kiSlider.value() - self.temp_ki = self.curr_ki + self.ki_edit_dist * slider_val / 100 - # if self.disp: print(self.temp_ki) - self.kiSpinBox.setValue(self.temp_ki) - - def kd_slider_callback(self): - slider_val = self.kdSlider.value() - self.temp_kd = self.curr_kd + self.kd_edit_dist * slider_val / 100 - # if self.disp: print(self.temp_kd) - self.kdSpinBox.setValue(self.temp_kd) - - - def set_SpinBoxes(self): - # Sliders have an integer range. Set this from +- 100 - self.kpSpinBox.setMinimum(self.curr_kp - self.kp_edit_dist) - self.kpSpinBox.setMaximum(self.curr_kp + self.kp_edit_dist) - self.kpSpinBox.setValue(self.curr_kp) - - self.kiSpinBox.setMinimum(self.curr_ki - self.ki_edit_dist) - self.kiSpinBox.setMaximum(self.curr_ki + self.ki_edit_dist) - self.kiSpinBox.setValue(self.curr_ki) - - self.kdSpinBox.setMinimum(self.curr_kd - self.kd_edit_dist) - self.kdSpinBox.setMaximum(self.curr_kd + self.kd_edit_dist) - self.kdSpinBox.setValue(self.curr_kd) - - def kpSpinBox_callback(self): - kpSpinBox_value = self.kpSpinBox.value() - self.temp_kp = kpSpinBox_value - slider_val = (self.temp_kp - self.curr_kp)*100/self.kp_edit_dist - self.kpSlider.setValue(int(slider_val)) - - def kiSpinBox_callback(self): - kiSpinBox_value = self.kiSpinBox.value() - self.temp_ki = kiSpinBox_value - slider_val = (self.temp_ki - self.curr_ki)*100/self.ki_edit_dist - self.kiSlider.setValue(int(slider_val)) - - def kdSpinBox_callback(self): - kdSpinBox_value = self.kdSpinBox.value() - self.temp_kd = kdSpinBox_value - slider_val = (self.temp_kd - self.curr_kd)*100/self.kd_edit_dist - self.kdSlider.setValue(int(slider_val)) - - - def runButtonCallback(self): - if self.disp: print('RUNNING parameters') - #call this if run button is pushed - # Set the undo values to be the current values - self.undo_kp = self.curr_kp - self.undo_ki = self.curr_ki - self.undo_kd = self.curr_kd - # Set current variables to be temp variables - self.curr_kp = self.temp_kp - self.curr_ki = self.temp_ki - self.curr_kd = self.temp_kd - #execute ros param set functions - executable1 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_kp", f"{self.curr_kp}"] - executable2 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_ki", f"{self.curr_ki}"] - executable3 = ["ros2", "param", "set", "/autopilot", f"{self.tuning_mode}_kd", f"{self.curr_kd}"] - executables = [executable1, executable2, executable3] - for executable in executables: - output = subprocess.run(executable, stdout=subprocess.PIPE) - if self.disp: print(output.stdout) - if self.disp: - print('Kp set to:', self.curr_kp) - print('Ki set to:', self.curr_ki) - print('Kd set to:', self.curr_kd) - - # Reinitialize the gui - self.set_sliders() - self.set_SpinBoxes() - - - def clearButtonCallback(self): #resets the current mode's inputs to original or last save values - if self.disp: print('\nCLEARING <' + self.tuning_mode + '> parameters') - params = ['p','i','d'] - for param in params: - orig_var_name = f"orig_{self.tuning_mode}_k{param}" - #get parameter values for orig_var_name - original_value = getattr(self,orig_var_name) - #generate curr param variable names - curr_var_name = f"temp_k{param}" - #Assign original values to curr parameters - setattr(self, curr_var_name, float(original_value)) - print(f'{curr_var_name} set to {original_value}') - #run button callback to apply changes - self.runButtonCallback() - - def saveButtonCallback(self): - if self.disp: print('\nSAVING all parameters') - modes = ["c", "p", "r", "a_t", "a"] - params = ['p', 'i', 'd'] - for mode in modes: - for param in params: - try: - #Rn this only calls the original param values and renames them - output = subprocess.run(["ros2", "param", "get", "/autopilot", f"{mode}_k{param}"], stdout=subprocess.PIPE, check=True) - output_list = output.stdout.split() - output_val = output_list[-1] - # Dynamically generate variable names and assign values - var_name = f"orig_{mode}_k{param}" - setattr(self, var_name, output_val) - print(f'{var_name} set to {output_val}') - except subprocess.CalledProcessError as e: - print(f"Failed to get parameter value for {mode}_k{param}: {e}") - -# Main loop -if __name__=='__main__': - app = QApplication(sys.argv) - window = Window() - window.show() - sys.exit(app.exec_()) diff --git a/rosplane_tuning/gui/tuning_gui.py b/rosplane_tuning/gui/tuning_gui.py deleted file mode 100644 index b907c51..0000000 --- a/rosplane_tuning/gui/tuning_gui.py +++ /dev/null @@ -1,148 +0,0 @@ -# -*- coding: utf-8 -*- - -# Form implementation generated from reading ui file 'tuning_gui.ui' -# -# Created by: PyQt5 UI code generator 5.15.9 -# -# WARNING: Any manual changes made to this file will be lost when pyuic5 is -# run again. Do not edit this file unless you know what you are doing. - - -from PyQt5 import QtCore, QtGui, QtWidgets - - -class Ui_MainWindow(object): - def setupUi(self, MainWindow): - MainWindow.setObjectName("MainWindow") - MainWindow.resize(810, 600) - sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Fixed, QtWidgets.QSizePolicy.Fixed) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(MainWindow.sizePolicy().hasHeightForWidth()) - MainWindow.setSizePolicy(sizePolicy) - self.centralwidget = QtWidgets.QWidget(MainWindow) - sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Preferred, QtWidgets.QSizePolicy.Preferred) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(self.centralwidget.sizePolicy().hasHeightForWidth()) - self.centralwidget.setSizePolicy(sizePolicy) - self.centralwidget.setObjectName("centralwidget") - self.horizontalLayoutWidget = QtWidgets.QWidget(self.centralwidget) - self.horizontalLayoutWidget.setGeometry(QtCore.QRect(20, 10, 761, 571)) - self.horizontalLayoutWidget.setObjectName("horizontalLayoutWidget") - self.horizontalLayout = QtWidgets.QHBoxLayout(self.horizontalLayoutWidget) - self.horizontalLayout.setSizeConstraint(QtWidgets.QLayout.SetFixedSize) - self.horizontalLayout.setContentsMargins(0, 0, 0, 0) - self.horizontalLayout.setSpacing(6) - self.horizontalLayout.setObjectName("horizontalLayout") - self.verticalLayout = QtWidgets.QVBoxLayout() - self.verticalLayout.setSizeConstraint(QtWidgets.QLayout.SetMaximumSize) - self.verticalLayout.setContentsMargins(-1, -1, 50, -1) - self.verticalLayout.setSpacing(6) - self.verticalLayout.setObjectName("verticalLayout") - self.CourseButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) - self.CourseButton.setObjectName("CourseButton") - self.verticalLayout.addWidget(self.CourseButton) - self.rollButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) - self.rollButton.setObjectName("rollButton") - self.verticalLayout.addWidget(self.rollButton) - self.pitchButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) - self.pitchButton.setObjectName("pitchButton") - self.verticalLayout.addWidget(self.pitchButton) - self.airspeedButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) - self.airspeedButton.setObjectName("airspeedButton") - self.verticalLayout.addWidget(self.airspeedButton) - self.altitudeButton = QtWidgets.QRadioButton(self.horizontalLayoutWidget) - self.altitudeButton.setObjectName("altitudeButton") - self.verticalLayout.addWidget(self.altitudeButton) - self.runButton = QtWidgets.QPushButton(self.horizontalLayoutWidget) - self.runButton.setObjectName("runButton") - self.verticalLayout.addWidget(self.runButton) - self.saveButton = QtWidgets.QPushButton(self.horizontalLayoutWidget) - self.saveButton.setObjectName("saveButton") - self.verticalLayout.addWidget(self.saveButton) - self.clearButton = QtWidgets.QPushButton(self.horizontalLayoutWidget) - self.clearButton.setObjectName("Clear") - self.verticalLayout.addWidget(self.clearButton) - self.horizontalLayout.addLayout(self.verticalLayout) - self.horizontalLayout_3 = QtWidgets.QHBoxLayout() - self.horizontalLayout_3.setObjectName("horizontalLayout_3") - self.verticalLayout_4 = QtWidgets.QVBoxLayout() - self.verticalLayout_4.setObjectName("verticalLayout_4") - self.label_2 = QtWidgets.QLabel(self.horizontalLayoutWidget) - self.label_2.setObjectName("label_2") - self.verticalLayout_4.addWidget(self.label_2) - self.label = QtWidgets.QLabel(self.horizontalLayoutWidget) - self.label.setObjectName("label") - self.verticalLayout_4.addWidget(self.label) - self.label_3 = QtWidgets.QLabel(self.horizontalLayoutWidget) - self.label_3.setObjectName("label_3") - self.verticalLayout_4.addWidget(self.label_3) - self.horizontalLayout_3.addLayout(self.verticalLayout_4) - self.verticalLayout_3 = QtWidgets.QVBoxLayout() - self.verticalLayout_3.setSizeConstraint(QtWidgets.QLayout.SetDefaultConstraint) - self.verticalLayout_3.setContentsMargins(0, -1, 0, -1) - self.verticalLayout_3.setObjectName("verticalLayout_3") - self.kpSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) - sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(self.kpSlider.sizePolicy().hasHeightForWidth()) - self.kpSlider.setSizePolicy(sizePolicy) - self.kpSlider.setMaximumSize(QtCore.QSize(400, 50)) - self.kpSlider.setOrientation(QtCore.Qt.Horizontal) - self.kpSlider.setObjectName("kpSlider") - self.verticalLayout_3.addWidget(self.kpSlider) - self.kiSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) - sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(self.kiSlider.sizePolicy().hasHeightForWidth()) - self.kiSlider.setSizePolicy(sizePolicy) - self.kiSlider.setMaximumSize(QtCore.QSize(400, 50)) - self.kiSlider.setOrientation(QtCore.Qt.Horizontal) - self.kiSlider.setObjectName("kiSlider") - self.verticalLayout_3.addWidget(self.kiSlider) - self.kdSlider = QtWidgets.QSlider(self.horizontalLayoutWidget) - sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Fixed) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(self.kdSlider.sizePolicy().hasHeightForWidth()) - self.kdSlider.setSizePolicy(sizePolicy) - self.kdSlider.setMaximumSize(QtCore.QSize(400, 50)) - self.kdSlider.setOrientation(QtCore.Qt.Horizontal) - self.kdSlider.setObjectName("kdSlider") - self.verticalLayout_3.addWidget(self.kdSlider) - self.horizontalLayout_3.addLayout(self.verticalLayout_3) - self.horizontalLayout.addLayout(self.horizontalLayout_3) - self.verticalLayout_5 = QtWidgets.QVBoxLayout() - self.verticalLayout_5.setObjectName("verticalLayout_5") - self.kpSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) - self.kpSpinBox.setObjectName("kpSpinBox") - self.verticalLayout_5.addWidget(self.kpSpinBox) - self.kiSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) - self.kiSpinBox.setObjectName("kiSpinBox") - self.verticalLayout_5.addWidget(self.kiSpinBox) - self.kdSpinBox = QtWidgets.QDoubleSpinBox(self.horizontalLayoutWidget) - self.kdSpinBox.setObjectName("kdSpinBox") - self.verticalLayout_5.addWidget(self.kdSpinBox) - self.horizontalLayout.addLayout(self.verticalLayout_5) - MainWindow.setCentralWidget(self.centralwidget) - - self.retranslateUi(MainWindow) - QtCore.QMetaObject.connectSlotsByName(MainWindow) - - def retranslateUi(self, MainWindow): - _translate = QtCore.QCoreApplication.translate - MainWindow.setWindowTitle(_translate("MainWindow", "Rosplane Tuning GUI")) - self.CourseButton.setText(_translate("MainWindow", "Course")) - self.rollButton.setText(_translate("MainWindow", "Roll")) - self.pitchButton.setText(_translate("MainWindow", "Pitch")) - self.airspeedButton.setText(_translate("MainWindow", "Airspeed")) - self.altitudeButton.setText(_translate("MainWindow", "Altitude")) - self.runButton.setText(_translate("MainWindow", "Run")) - self.clearButton.setText(_translate("MainWindow", "Clear")) - self.saveButton.setText(_translate("MainWindow", "Save")) - self.label_2.setText(_translate("MainWindow", "k_p")) - self.label.setText(_translate("MainWindow", "k_i")) - self.label_3.setText(_translate("MainWindow", "k_d")) diff --git a/rosplane_tuning/resource/tuning_gui.ui b/rosplane_tuning/resource/tuning_gui.ui index c707041..120639b 100644 --- a/rosplane_tuning/resource/tuning_gui.ui +++ b/rosplane_tuning/resource/tuning_gui.ui @@ -6,8 +6,8 @@ 0 0 - 854 - 658 + 995 + 856 @@ -52,45 +52,106 @@ 16777215 - + - 10 - 10 - 791 - 611 + -1 + -1 + 811 + 631 - - - QLayout::SetDefaultConstraint - - - - + + + + + Qt::Horizontal + + + QSizePolicy::Maximum + + - 16777215 - 50 + 10 + 20 - - <html><head/><body><p align="center"></p><p align="center"><span style=" font-size:16pt;">ROSplane Tuning GUI</span></p></body></html> + + + + + + QFrame::Plain + + + 2 + + + Qt::Horizontal - - - - 6 - + + + + + + + 16777215 + 50 + + + + <html><head/><body><p align="center"></p><p align="center"><span style=" font-size:16pt;">ROSplane Tuning GUI</span></p></body></html> + + + + + + + + 150 + 0 + + + + + 200 + 16777215 + + + + false + + + QFrame::Box + + + QFrame::Plain + + + 2 + + + 0 + + + RC Override Active + + + Qt::AlignCenter + + + + + + + - QLayout::SetDefaultConstraint - - - 0 + QLayout::SetFixedSize - + 6 @@ -98,395 +159,705 @@ QLayout::SetDefaultConstraint - 50 + 0 - - - Course - - - - - - - Roll + + + Qt::Horizontal - - - - - - Pitch - - - - - - - Airspeed + + QSizePolicy::Maximum - - - - - - Altitude + + + 10 + 20 + - + - - - Run + + + 0 - - - - - - - 0 - 0 - + + QLayout::SetDefaultConstraint - - - 0 - 0 - + + 10 - - Clear + + 10 - - - - - - - - QLayout::SetDefaultConstraint - - - 0 - - - 0 - - - - - - - - - 70 - 16777215 - - - - <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">p </span><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 0 - - - - - - - - - - QFrame::Plain - - - 1 - - - Qt::Vertical - - - - - - - 10 - - - - - Previous Value: - - - - - - - Saved Value: - - - - - - + + + + 16777215 + 40 + + + + Mode Select + + - - - - 0 - 0 - - + 16777215 - 40 + 10 + + QFrame::Plain + + + 1 + Qt::Horizontal - - QSlider::TicksBelow + + + + + + Course + + + + + + + Roll - - 0 + + + + + + Pitch - - - Qt::Vertical + + + Airspeed - - QSizePolicy::Maximum + + + + + + Altitude + + + + + + + Run + + + + + + + Undo + + + + + + + + 0 + 0 + - + - 20 - 40 + 0 + 0 - + + Save + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + Clear + + - + + + QLayout::SetDefaultConstraint + + + 0 + + + 0 + - + - + + + + + + 70 + 16777215 + + + + <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">p </span><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 0 + + + + + + + + + + QFrame::Plain + + + 1 + + + Qt::Vertical + + + + + + + 10 + + + + + Previous Value: + + + + + + + Saved Value: + + + + + + + + + + + + 0 + 0 + + - 70 - 16777215 + 16777215 + 40 - - <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">i </span><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> + + Qt::Horizontal - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + QSlider::TicksBelow - + 0 - - - - - - QFrame::Plain - - - 1 - + Qt::Vertical - + + QSizePolicy::Maximum + + + + 20 + 100 + + + + + + + - - - 10 - + - + + + + 70 + 16777215 + + - Previous Value: + <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">i </span><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 0 - - - Saved Value: + + + + + + QFrame::Plain + + + 1 + + + Qt::Vertical + + + + 10 + + + + + Previous Value: + + + + + + + Saved Value: + + + + + - - - - - - - 0 - 0 - - - - - 16777215 - 40 - - - - Qt::Horizontal - - - QSlider::TicksBelow - - - - - - - Qt::Vertical - - - QSizePolicy::Maximum - - - - 20 - 40 - - - - - - - - - - - + + + + 0 + 0 + + - 70 - 16777215 + 16777215 + 40 - - <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">d</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;"/><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + Qt::Horizontal - - 0 + + QSlider::TicksBelow - - - - - - QFrame::Plain - - - 1 - + Qt::Vertical - + + QSizePolicy::Maximum + + + + 20 + 100 + + + + + + + - - - 10 - + - + + + + 70 + 16777215 + + - Previous Value: + <html><head/><body><p><span style=" font-size:14pt;">k</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;">d</span><span style=" font-size:14pt; font-style:italic; vertical-align:sub;"/><span style=" font-size:14pt; font-style:italic;">=</span></p></body></html> + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 0 - - - Saved Value: + + + + + + QFrame::Plain + + + 1 + + + Qt::Vertical + + + + 10 + + + + + Previous Value: + + + + + + + Saved Value: + + + + + + + + + + 0 + 0 + + + + + 16777215 + 40 + + + + Qt::Horizontal + + + QSlider::TicksBelow + + + + + + + Qt::Vertical + + + QSizePolicy::Maximum + + + + 20 + 100 + + + + - - - - - 0 - 0 - - - - - 16777215 - 40 - - - - Qt::Horizontal - - - QSlider::TicksBelow - - - - - - - Qt::Vertical - - - QSizePolicy::Maximum - - - - 20 - 40 - - - - + + + + 0 + + + + + + 100 + 0 + + + + + 16777215 + 60 + + + + Current +Parameter +Values + + + Qt::AlignCenter + + + + + + + + 16777215 + 20 + + + + false + + + background-color: #b3b3b3 + + + QFrame::NoFrame + + + COURSE: + + + Qt::AlignCenter + + + + + + + + 16777215 + 65 + + + + c_kp: 0.0 +c_ki: 0.0 +c_kd: 0.0 + + + + + + + + 16777215 + 20 + + + + background-color: #b3b3b3 + + + ROLL: + + + Qt::AlignCenter + + + + + + + + 16777215 + 65 + + + + r_kp: 0.0 +r_ki: 0.0 +r_kd: 0.0 + + + + + + + + 16777215 + 20 + + + + background-color: #b3b3b3 + + + PITCH: + + + Qt::AlignCenter + + + + + + + + 16777215 + 65 + + + + p_kp: 0.0 +p_ki: 0.0 +p_kd: 0.0 + + + + + + + + 16777215 + 20 + + + + background-color: #b3b3b3 + + + AIRSPEED: + + + Qt::AlignCenter + + + + + + + + 16777215 + 65 + + + + a_t_kp: 0.0 +a_t_ki: 0.0 +a_t_kd: 0.0 + + + + + + + + 16777215 + 20 + + + + background-color: #b3b3b3 + + + ALTITUDE: + + + Qt::AlignCenter + + + + + + + + 16777215 + 65 + + + + a_kp: 0.0 +a_ki: 0.0 +a_kd: 0.0 + + + + + + + + + QFrame::Plain + + + 2 + + + Qt::Vertical + + + diff --git a/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py b/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py index de11f57..36985e5 100644 --- a/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py +++ b/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py @@ -1,18 +1,60 @@ import os import rclpy +from rclpy import qos +from rclpy.node import Node +from rcl_interfaces.srv import SetParameters, GetParameters +from rcl_interfaces.msg import Parameter, ParameterValue, ParameterType, ParameterEvent + +from rosflight_msgs.msg import Status from qt_gui.plugin import Plugin from python_qt_binding import loadUi from python_qt_binding.QtWidgets import QMainWindow, QWidget, QSizePolicy, QHBoxLayout, QVBoxLayout, QRadioButton, QPushButton, QLabel, QLayout, QSlider, QDoubleSpinBox -from python_qt_binding.QtCore import QSize, Qt, QRect +from python_qt_binding import QtCore, QtGui from ament_index_python import get_resource +class ParameterClient(Node): + def __init__(self): + super().__init__('tuning_gui_parameter_client') + + self.get_client = self.create_client(GetParameters, '/autopilot/get_parameters') + while not self.get_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('GET_PARAMETERS service not available, waiting again... is ROSplane autopilot running?') + self.get_req = GetParameters.Request() + + self.set_client = self.create_client(SetParameters, '/autopilot/set_parameters') + while not self.get_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('SET_PARAMETERS service not available, waiting again... is ROSplane autopilot running?') + self.set_req = SetParameters.Request() + + def get_autopilot_params(self, param_names:list[str]): + self.get_req.names = param_names + self.get_future = self.get_client.call_async(self.get_req) + rclpy.spin_until_future_complete(self, self.get_future) + return self.get_future.result() + + def set_autopilot_params(self, param_names:list[str], param_values:list[float]): + assert len(param_values) == len(param_names) + # Create list of Parameter objects + parameters = [] + for i in range(len(param_names)): + newParamValue = ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=param_values[i]) + parameters.append(Parameter(name=param_names[i], value=newParamValue)) + self.set_req.parameters = parameters + self.set_future = self.set_client.call_async(self.set_req) + rclpy.spin_until_future_complete(self, self.set_future) + return self.set_future.result() + + class ROSflightGUI(Plugin): + colorChanged = QtCore.pyqtSignal(QtGui.QColor) + def __init__(self, context): super(ROSflightGUI, self).__init__(context) # Give QObjects reasonable names self.setObjectName('ROSflightGUI') + self._context = context # Process standalone plugin command-line arguments from argparse import ArgumentParser @@ -45,7 +87,83 @@ def __init__(self, context): # Add widget to the user interface context.add_widget(self._widget) - # self.setupUi() + # Create ROS2 node to do node things + self._node = ParameterClient() + self.rc_override = False + self._rc_override_sub = self._context.node.create_subscription(Status, '/status', self.status_sub_callback, qos.qos_profile_sensor_data) + self._param_sub = self._context.node.create_subscription(ParameterEvent, '/parameter_events', self.parameter_event_callback, 10) + + # Set up the UI + self.setup_UI() + + + def setup_UI(self): + self.initialize_gui() + self.connectSignalSlots() + self.set_sizes() + + def initialize_gui(self): + self.tuning_mode = '' + self.curr_kp = 0.0 + self.curr_kd = 0.0 + self.curr_ki = 0.0 + + self.temp_kp = 0.0 + self.temp_kd = 0.0 + self.temp_ki = 0.0 + + self.undo_kp = self.curr_kp + self.undo_kd = self.curr_kd + self.undo_ki = self.curr_ki + # This allows us to have different ranges for fine tuning kp, ki, and kd + self.kp_edit_dist = 2.0 + self.ki_edit_dist = 0.5 + self.kd_edit_dist = 2.0 + # Boolean values for controlling debugging statements + self.time = False + self.disp = True + + # Original parameters saved at init, called with clear button + self.call_originals() + + # Strings for the readout list + self.ckp = self.orig_c_kp + self.cki = self.orig_c_ki + self.ckd = self.orig_c_kd + self.rkp = self.orig_r_kp + self.rki = self.orig_r_ki + self.rkd = self.orig_r_kd + self.pkp = self.orig_p_kp + self.pki = self.orig_p_ki + self.pkd = self.orig_p_kd + self.akp = self.orig_a_kp + self.aki = self.orig_a_ki + self.akd = self.orig_a_kd + self.a_t_kp = self.orig_a_t_kp + self.a_t_ki = self.orig_a_t_ki + self.a_t_kd = self.orig_a_t_kd + + def call_originals(self): + (self.orig_c_kp, self.orig_c_ki, self.orig_c_kd) = self.get_params(['c_kp', 'c_ki', 'c_kd']) + (self.orig_p_kp, self.orig_p_ki, self.orig_p_kd) = self.get_params(['p_kp', 'p_ki', 'p_kd']) + (self.orig_r_kp, self.orig_r_ki, self.orig_r_kd) = self.get_params(['r_kp', 'r_ki', 'r_kd']) + (self.orig_a_t_kp, self.orig_a_t_ki, self.orig_a_t_kd) = self.get_params(['a_t_kp', 'a_t_ki', 'a_t_kd']) + (self.orig_a_kp, self.orig_a_ki, self.orig_a_kd) = self.get_params(['a_kp', 'a_ki', 'a_kd']) + + + def set_sizes(self): + self._widget.kpSlider.setMinimum(-100) + self._widget.kpSlider.setMaximum(100) + self._widget.kiSlider.setMinimum(-100) + self._widget.kiSlider.setMaximum(100) + self._widget.kdSlider.setMinimum(-100) + self._widget.kdSlider.setMaximum(100) + self._widget.kpSpinBox.setSingleStep(0.001) + self._widget.kiSpinBox.setSingleStep(0.001) + self._widget.kdSpinBox.setSingleStep(0.001) + self._widget.kpSpinBox.setDecimals(3) + self._widget.kiSpinBox.setDecimals(3) + self._widget.kdSpinBox.setDecimals(3) def shutdown_plugin(self): # TODO unregister all publishers here @@ -66,111 +184,348 @@ def restore_settings(self, plugin_settings, instance_settings): # This will enable a setting button (gear icon) in each dock widget title bar # Usually used to open a modal configuration dialog - def setupUi(self): - self.centralwidget = QWidget(self._widget) - sizePolicy = QSizePolicy(QSizePolicy.Preferred, QSizePolicy.Preferred) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(self.centralwidget.sizePolicy().hasHeightForWidth()) - self.centralwidget.setSizePolicy(sizePolicy) - self.centralwidget.setObjectName("centralwidget") - self.horizontalLayoutWidget = QWidget(self.centralwidget) - self.horizontalLayoutWidget.setGeometry(QRect(20, 10, 761, 571)) - self.horizontalLayoutWidget.setObjectName("horizontalLayoutWidget") - self.horizontalLayout = QHBoxLayout(self.horizontalLayoutWidget) - self.horizontalLayout.setSizeConstraint(QLayout.SetFixedSize) - self.horizontalLayout.setContentsMargins(0, 0, 0, 0) - self.horizontalLayout.setSpacing(6) - self.horizontalLayout.setObjectName("horizontalLayout") - self.verticalLayout = QVBoxLayout() - self.verticalLayout.setSizeConstraint(QLayout.SetMaximumSize) - self.verticalLayout.setContentsMargins(-1, -1, 50, -1) - self.verticalLayout.setSpacing(6) - self.verticalLayout.setObjectName("verticalLayout") - self.CourseButton = QRadioButton(self.horizontalLayoutWidget) - self.CourseButton.setObjectName("CourseButton") - self.verticalLayout.addWidget(self.CourseButton) - self.rollButton = QRadioButton(self.horizontalLayoutWidget) - self.rollButton.setObjectName("rollButton") - self.verticalLayout.addWidget(self.rollButton) - self.pitchButton = QRadioButton(self.horizontalLayoutWidget) - self.pitchButton.setObjectName("pitchButton") - self.verticalLayout.addWidget(self.pitchButton) - self.airspeedButton = QRadioButton(self.horizontalLayoutWidget) - self.airspeedButton.setObjectName("airspeedButton") - self.verticalLayout.addWidget(self.airspeedButton) - self.altitudeButton = QRadioButton(self.horizontalLayoutWidget) - self.altitudeButton.setObjectName("altitudeButton") - self.verticalLayout.addWidget(self.altitudeButton) - self.runButton = QPushButton(self.horizontalLayoutWidget) - self.runButton.setObjectName("runButton") - self.verticalLayout.addWidget(self.runButton) - self.saveButton = QPushButton(self.horizontalLayoutWidget) - self.saveButton.setObjectName("saveButton") - self.verticalLayout.addWidget(self.saveButton) - self.clearButton = QPushButton(self.horizontalLayoutWidget) - self.clearButton.setObjectName("Clear") - self.verticalLayout.addWidget(self.clearButton) - self.horizontalLayout.addLayout(self.verticalLayout) - self.horizontalLayout_3 = QHBoxLayout() - self.horizontalLayout_3.setObjectName("horizontalLayout_3") - self.verticalLayout_4 = QVBoxLayout() - self.verticalLayout_4.setObjectName("verticalLayout_4") - self.label_2 = QLabel(self.horizontalLayoutWidget) - self.label_2.setObjectName("label_2") - self.verticalLayout_4.addWidget(self.label_2) - self.label = QLabel(self.horizontalLayoutWidget) - self.label.setObjectName("label") - self.verticalLayout_4.addWidget(self.label) - self.label_3 = QLabel(self.horizontalLayoutWidget) - self.label_3.setObjectName("label_3") - self.verticalLayout_4.addWidget(self.label_3) - self.horizontalLayout_3.addLayout(self.verticalLayout_4) - self.verticalLayout_3 = QVBoxLayout() - self.verticalLayout_3.setSizeConstraint(QLayout.SetDefaultConstraint) - self.verticalLayout_3.setContentsMargins(0, -1, 0, -1) - self.verticalLayout_3.setObjectName("verticalLayout_3") - self.kpSlider = QSlider(self.horizontalLayoutWidget) - sizePolicy = QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(self.kpSlider.sizePolicy().hasHeightForWidth()) - self.kpSlider.setSizePolicy(sizePolicy) - self.kpSlider.setMaximumSize(QSize(400, 50)) - self.kpSlider.setOrientation(Qt.Horizontal) - self.kpSlider.setObjectName("kpSlider") - self.verticalLayout_3.addWidget(self.kpSlider) - self.kiSlider = QSlider(self.horizontalLayoutWidget) - sizePolicy = QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(self.kiSlider.sizePolicy().hasHeightForWidth()) - self.kiSlider.setSizePolicy(sizePolicy) - self.kiSlider.setMaximumSize(QSize(400, 50)) - self.kiSlider.setOrientation(Qt.Horizontal) - self.kiSlider.setObjectName("kiSlider") - self.verticalLayout_3.addWidget(self.kiSlider) - self.kdSlider = QSlider(self.horizontalLayoutWidget) - sizePolicy = QSizePolicy(QSizePolicy.Expanding, QSizePolicy.Fixed) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(self.kdSlider.sizePolicy().hasHeightForWidth()) - self.kdSlider.setSizePolicy(sizePolicy) - self.kdSlider.setMaximumSize(QSize(400, 50)) - self.kdSlider.setOrientation(Qt.Horizontal) - self.kdSlider.setObjectName("kdSlider") - self.verticalLayout_3.addWidget(self.kdSlider) - self.horizontalLayout_3.addLayout(self.verticalLayout_3) - self.horizontalLayout.addLayout(self.horizontalLayout_3) - self.verticalLayout_5 = QVBoxLayout() - self.verticalLayout_5.setObjectName("verticalLayout_5") - self.kpSpinBox = QDoubleSpinBox(self.horizontalLayoutWidget) - self.kpSpinBox.setObjectName("kpSpinBox") - self.verticalLayout_5.addWidget(self.kpSpinBox) - self.kiSpinBox = QDoubleSpinBox(self.horizontalLayoutWidget) - self.kiSpinBox.setObjectName("kiSpinBox") - self.verticalLayout_5.addWidget(self.kiSpinBox) - self.kdSpinBox = QDoubleSpinBox(self.horizontalLayoutWidget) - self.kdSpinBox.setObjectName("kdSpinBox") - self.verticalLayout_5.addWidget(self.kdSpinBox) - self.horizontalLayout.addLayout(self.verticalLayout_5) \ No newline at end of file + def connectSignalSlots(self): + # This is where we define signal slots (callbacks) for when the buttons get clicked + self._widget.CourseButton.toggled.connect(self.courseButtonCallback) + self._widget.pitchButton.toggled.connect(self.pitchButtonCallback) + self._widget.rollButton.toggled.connect(self.rollButtonCallback) + self._widget.airspeedButton.toggled.connect(self.airspeedButtonCallback) + self._widget.altitudeButton.toggled.connect(self.altitudeButtonCallback) + self._widget.runButton.clicked.connect(self.runButtonCallback) + self._widget.clearButton.clicked.connect(self.clearButtonCallback) + self._widget.saveButton.clicked.connect(self.saveButtonCallback) + self._widget.undoButton.clicked.connect(self.undoButtonCallback) + self._widget.kpSlider.valueChanged.connect(self.kp_slider_callback) + self._widget.kiSlider.valueChanged.connect(self.ki_slider_callback) + self._widget.kdSlider.valueChanged.connect(self.kd_slider_callback) + self._widget.kpSpinBox.valueChanged.connect(self.kpSpinBox_callback) + self._widget.kiSpinBox.valueChanged.connect(self.kiSpinBox_callback) + self._widget.kdSpinBox.valueChanged.connect(self.kdSpinBox_callback) + # Color changer for the RC Override box + self.colorChanged.connect(self.on_color_change) + + def courseButtonCallback(self): + if self._widget.CourseButton.isChecked(): + if self.disp: print("COURSE gains selected") + # Set the tuning mode + self.tuning_mode = 'c' + # Get the other parameters from ROS + (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) + # Set the sliders to the appropriate values + self.set_sliders() + self.set_SpinBoxes() + self.set_previousVals() + + def rollButtonCallback(self): + if self._widget.rollButton.isChecked(): + if self.disp: print("ROLL gains selected") + # Set the tuning mode + self.tuning_mode = 'r' + # Get the other parameters from ROS + (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) + self.set_sliders() + self.set_SpinBoxes() + self.set_previousVals() + + def pitchButtonCallback(self): + if self._widget.pitchButton.isChecked(): + if self.disp: print("PITCH gains selected") + # Set the tuning mode + self.tuning_mode = 'p' + # Get the other parameters from ROS + (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) + self.set_sliders() + self.set_SpinBoxes() + self.set_previousVals() + + def airspeedButtonCallback(self): + if self._widget.airspeedButton.isChecked(): + if self.disp: print("AIRSPEED gains selected") + # Set the tuning mode + self.tuning_mode = 'a_t' + # Get the other parameters from ROS + (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) + self.set_sliders() + self.set_SpinBoxes() + self.set_previousVals() + + def altitudeButtonCallback(self): + if self._widget.altitudeButton.isChecked(): + if self.disp: print("ALTITUDE gains selected") + # Set the tuning mode + self.tuning_mode = 'a' + # Get the other parameters from ROS + (self.curr_kp, self.curr_ki, self.curr_kd) = self.get_params([self.tuning_mode + '_kp', self.tuning_mode + '_ki', self.tuning_mode + '_kd']) + self.set_sliders() + self.set_SpinBoxes() + self.set_previousVals() + + def get_params(self, param_names:list[str]): + response = self._node.get_autopilot_params(param_names) + return [par_val.double_value for par_val in response.values] + + def set_sliders(self): + # Sliders have an integer range. Set this from +- 100 + self._widget.kpSlider.setValue(0) + self._widget.kiSlider.setValue(0) + self._widget.kdSlider.setValue(0) + + def set_SpinBoxes(self): + # Sliders have an integer range. Set this from +- 100 + self._widget.kpSpinBox.setMinimum(self.curr_kp - self.kp_edit_dist) + self._widget.kpSpinBox.setMaximum(self.curr_kp + self.kp_edit_dist) + self._widget.kpSpinBox.setValue(self.curr_kp) + + self._widget.kiSpinBox.setMinimum(self.curr_ki - self.ki_edit_dist) + self._widget.kiSpinBox.setMaximum(self.curr_ki + self.ki_edit_dist) + self._widget.kiSpinBox.setValue(self.curr_ki) + + self._widget.kdSpinBox.setMinimum(self.curr_kd - self.kd_edit_dist) + self._widget.kdSpinBox.setMaximum(self.curr_kd + self.kd_edit_dist) + self._widget.kdSpinBox.setValue(self.curr_kd) + + def kp_slider_callback(self): + slider_val = self._widget.kpSlider.value() + self.temp_kp = self.curr_kp + self.kp_edit_dist * slider_val / 100 + # if self.disp: print(self.temp_kp) + self._widget.kpSpinBox.setValue(self.temp_kp) + + def ki_slider_callback(self): + slider_val = self._widget.kiSlider.value() + self.temp_ki = self.curr_ki + self.ki_edit_dist * slider_val / 100 + # if self.disp: print(self.temp_ki) + self._widget.kiSpinBox.setValue(self.temp_ki) + + def kd_slider_callback(self): + slider_val = self._widget.kdSlider.value() + self.temp_kd = self.curr_kd + self.kd_edit_dist * slider_val / 100 + # if self.disp: print(self.temp_kd) + self._widget.kdSpinBox.setValue(self.temp_kd) + + def kpSpinBox_callback(self): + kpSpinBox_value = self._widget.kpSpinBox.value() + self.temp_kp = kpSpinBox_value + # slider_val = (self.temp_kp - self.curr_kp)*100/self.kp_edit_dist + # self._widget.kpSlider.setValue(int(slider_val)) + + def kiSpinBox_callback(self): + kiSpinBox_value = self._widget.kiSpinBox.value() + self.temp_ki = kiSpinBox_value + # slider_val = (self.temp_ki - self.curr_ki)*100/self.ki_edit_dist + # self._widget.kiSlider.setValue(int(slider_val)) + + def kdSpinBox_callback(self): + kdSpinBox_value = self._widget.kdSpinBox.value() + self.temp_kd = kdSpinBox_value + # slider_val = (self.temp_kd - self.curr_kd)*100/self.kd_edit_dist + # self._widget.kdSlider.setValue(int(slider_val)) + + def runButtonCallback(self, *args, mode=None): + # call this if run button is pushed + # Set the undo values to be the current values + self.undo_kp = self.curr_kp + self.undo_ki = self.curr_ki + self.undo_kd = self.curr_kd + # Set current variables to be temp variables + self.curr_kp = self.temp_kp + self.curr_ki = self.temp_ki + self.curr_kd = self.temp_kd + #execute ros param set functions + if mode is not None: + tun_mode = mode + else: + tun_mode = self.tuning_mode + + if self.disp: print('RUNNING parameters') + + param_names = [tun_mode+'_kp', tun_mode+'_ki', tun_mode+'_kd'] + print(param_names) + param_vals = [self.curr_kp, self.curr_ki, self.curr_kd] + result = self._node.set_autopilot_params(param_names, param_vals) + + for msg in result.results: + if not msg.successful: + print(f'WARNING: {msg} was not successful') + + if self.disp: + print('Kp set to:', self.curr_kp) + print('Ki set to:', self.curr_ki) + print('Kd set to:', self.curr_kd) + + # Reinitialize the gui + self.set_sliders() + self.set_SpinBoxes() + self.set_previousVals() + + # Resets the current mode's inputs to original or last save values + def clearButtonCallback(self): + # for mode in ['c', 'p', 'r', 'a', 'a_t']: + # if self.disp: print('\nCLEARING <' + mode + '> parameters') + # params = ['p','i','d'] + # for param in params: + # orig_var_name = f"orig_{mode}_k{param}" + # #get parameter values for orig_var_name + # original_value = getattr(self,orig_var_name) + # #generate curr param variable names + # curr_var_name = f"temp_k{param}" + # #Assign original values to curr parameters + # setattr(self, curr_var_name, float(original_value)) + # print(f'{curr_var_name} set to {original_value}') + # #run button callback to apply changes + # self.tuning_mode = mode + # self.runButtonCallback() + # reset current values to prevent the undos from being wild + self.curr_kd = 0.0 + self.curr_ki = 0.0 + self.curr_kp = 0.0 + + temporary_kp = 0.0 + temporary_ki = 0.0 + temporary_kd = 0.0 + + self.temp_kp = self.orig_c_kp + self.temp_ki = self.orig_c_ki + self.temp_kd = self.orig_c_kd + if self.tuning_mode == 'c': + temporary_kp = self.temp_kp + temporary_ki = self.temp_ki + temporary_kd = self.temp_kd + self.runButtonCallback(mode='c') + + self.temp_kp = self.orig_r_kp + self.temp_ki = self.orig_r_ki + self.temp_kd = self.orig_r_kd + if self.tuning_mode == 'r': + temporary_kp = self.temp_kp + temporary_ki = self.temp_ki + temporary_kd = self.temp_kd + self.runButtonCallback(mode='r') + + self.temp_kp = self.orig_p_kp + self.temp_ki = self.orig_p_ki + self.temp_kd = self.orig_p_kd + if self.tuning_mode == 'p': + temporary_kp = self.temp_kp + temporary_ki = self.temp_ki + temporary_kd = self.temp_kd + self.runButtonCallback(mode='p') + + self.temp_kp = self.orig_a_kp + self.temp_ki = self.orig_a_ki + self.temp_kd = self.orig_a_kd + if self.tuning_mode == 'a': + temporary_kp = self.temp_kp + temporary_ki = self.temp_ki + temporary_kd = self.temp_kd + self.runButtonCallback(mode='a') + + self.temp_kp = self.orig_a_t_kp + self.temp_ki = self.orig_a_t_ki + self.temp_kd = self.orig_a_t_kd + if self.tuning_mode == 'a_t': + temporary_kp = self.temp_kp + temporary_ki = self.temp_ki + temporary_kd = self.temp_kd + self.runButtonCallback(mode='a_t') + + # Reset the current values on the sliders to match the current tuning mode + self.temp_kp = temporary_kp + self.temp_ki = temporary_ki + self.temp_kd = temporary_kd + self.runButtonCallback() + + def saveButtonCallback(self): + if self.disp: print('\nSAVING all parameters') + self.call_originals() + self.clearButtonCallback() + + def undoButtonCallback(self): + self.temp_kp = self.undo_kp + self.temp_ki = self.undo_ki + self.temp_kd = self.undo_kd + self.runButtonCallback() + + def set_previousVals(self): + self._widget.kp_prev_val.setText(f'Previous Value: {self.undo_kp}') + self._widget.ki_prev_val.setText(f'Previous Value: {self.undo_ki}') + self._widget.kd_prev_val.setText(f'Previous Value: {self.undo_kd}') + if self.tuning_mode == 'p': + self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_p_kp}') + self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_p_ki}') + self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_p_kd}') + elif self.tuning_mode == 'c': + self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_c_kp}') + self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_c_ki}') + self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_c_kd}') + elif self.tuning_mode == 'r': + self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_r_kp}') + self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_r_ki}') + self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_r_kd}') + elif self.tuning_mode == 'a': + self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_a_kp}') + self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_a_ki}') + self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_a_kd}') + elif self.tuning_mode == 'a_t': + self._widget.kp_saved_val.setText(f'Saved Value: {self.orig_a_t_kp}') + self._widget.ki_saved_val.setText(f'Saved Value: {self.orig_a_t_ki}') + self._widget.kd_saved_val.setText(f'Saved Value: {self.orig_a_t_kd}') + + def status_sub_callback(self, msg): + if msg.rc_override: + if not self.rc_override: + self.rc_override = True + print('RC OVERRIDE ?!?!?!?') + self.colorChanged.emit(QtGui.QColor(219, 44, 44)) + # self._widget.rcOverrideLabel.setStyleSheet("QLabel {background-color: red; color: white}") + else: + if self.rc_override: + self.rc_override = False + print('not rc override') + self.colorChanged.emit(QtGui.QColor(87, 225, 97)) + # self._widget.rcOverrideLabel.setStyleSheet("QLabel {background-color: green; color: white}") + + @QtCore.pyqtSlot(QtGui.QColor) + def on_color_change(self, color): + self._widget.rcOverrideLabel.setStyleSheet("QLabel {background-color: " + color.name() + "; color: white}") + + def parameter_event_callback(self,msg): + # Look only at the autopilot node + if msg.node == '/autopilot': + for param in msg.changed_parameters: + if param.name == 'c_kp': + self.ckp = param.value.double_value + elif param.name == 'c_ki': + self.cki = param.value.double_value + elif param.name == 'c_kd': + self.ckd = param.value.double_value + elif param.name == 'r_kp': + self.rkp = param.value.double_value + elif param.name == 'r_ki': + self.rki = param.value.double_value + elif param.name == 'r_kd': + self.rkd = param.value.double_value + elif param.name == 'p_kp': + self.pkp = param.value.double_value + elif param.name == 'p_ki': + self.pki = param.value.double_value + elif param.name == 'p_kd': + self.pkd = param.value.double_value + elif param.name == 'a_kp': + self.akp = param.value.double_value + elif param.name == 'a_ki': + self.aki = param.value.double_value + elif param.name == 'a_kd': + self.akd = param.value.double_value + elif param.name == 'a_t_kp': + self.a_t_kp = param.value.double_value + elif param.name == 'a_t_ki': + self.a_t_ki = param.value.double_value + elif param.name == 'a_t_kd': + self.a_t_kd = param.value.double_value + + # Update the text boxes. If updating all of them is too slow, set them one at a time + self._widget.courseReadout.setText(f'c_kp: {self.ckp}\nc_ki: {self.cki}\nc_kd: {self.ckd}') + self._widget.rollReadout.setText(f'r_kp: {self.rkp}\nr_ki: {self.rki}\nr_kd: {self.rkd}') + self._widget.pitchReadout.setText(f'p_kp: {self.pkp}\np_ki: {self.pki}\np_kd: {self.pkd}') + self._widget.altitudeReadout.setText(f'a_kp: {self.akp}\na_ki: {self.aki}\na_kd: {self.akd}') + self._widget.airspeedReadout.setText(f'a_t_kp: {self.a_t_kp}\na_t_ki: {self.a_t_ki}\na_t_kd: {self.a_t_kd}') \ No newline at end of file