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