diff --git a/README.md b/README.md index c816fa5..c8df6e8 100644 --- a/README.md +++ b/README.md @@ -30,13 +30,13 @@ To run ROSplane in simulation do the following: 1. Launch Gazebo and firmware simulation with `ros2 launch rosflight_sim fixedwing.launch.py` 2. Next, run the rosflight_io node configured for SIL with `ros2 run rosflight_io rosflight_io --ros-args -p udp:=true` -3. Update the firmware parameters for fixedwing flight with `ros2 service call /param_load_from_file rosflight_msgs/srv/ParamFile "filename: rosflight2/rosflight_utils/params/fixedwing_param.yml"` if this is the first time launching. +3. Update the firmware parameters for fixedwing flight with `ros2 service call /param_load_from_file rosflight_msgs/srv/ParamFile "filename: rosflight/rosflight_sim/params/fixedwing_firmware.yaml"` if this is the first time launching. 4. Write the new parameters to memory for convenience with `ros2 service call /param_write std_srvs/srv/Trigger` - Note: the firmware will time out and not allow takeoff after 100 seconds, so you may need to redo steps one and two. 5. Calibrate the IMU to allow the airplane to be armed with `ros2 service call /calibrate_imu std_srvs/srv/Trigger` 6. Then launch ROSplane with `ros2 launch rosplane_sim sim.launch.py`, this will run with the default controller. To use the total energy controller add the argument `control_type:=total_energy`. 7. Connect a controller with `ros2 run rosflight_utils rc_joy.py --ros-args --remap /RC:=/fixedwing/RC` or if you do not want to use a controller, run `python3 rc_sim.py --ros-args --remap RC:=/fixedwing/RC` while inside `rosflight2/rosflight_utils/src`. -8. Finally, arm the aircraft with channel 4 and then disable throttle and attitude override with channel 5. If not using a controller use `ros2 service call /arm std_srvs/srv/Trigger` and then disable RC override with `ros2 service call /disable_override std_srvs/srv/Trigger`. +8. Finally, arm the aircraft with channel 4 and then disable throttle and attitude override with channel 5. If not using a controller use `ros2 service call /toggle_arm std_srvs/srv/Trigger` and then disable RC override with `ros2 service call /toggle_overide std_srvs/srv/Trigger`. 9. The aircraft should now be airborne! ## Running ROSplane on hardware. diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp index 1ee1e10..7788292 100644 --- a/rosplane/src/path_manager_base.cpp +++ b/rosplane/src/path_manager_base.cpp @@ -41,6 +41,8 @@ PathManagerBase::PathManagerBase() void PathManagerBase::declare_parameters() { params_.declare_double("R_min", 50.0); params_.declare_double("current_path_pub_frequency", 100.0); + params_.declare_double("default_altitude", 50.0); + params_.declare_double("default_airspeed", 15.0); } void PathManagerBase::set_timer() { @@ -91,6 +93,7 @@ void PathManagerBase::vehicle_state_callback(const rosplane_msgs::msg::State & m void PathManagerBase::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & msg) { double R_min = params_.get_double("R_min"); + double default_altitude = params_.get_double("default_altitude"); orbit_dir_ = 0; // If the message contains "clear_wp_list", then clear all waypoints and do nothing else @@ -109,7 +112,14 @@ void PathManagerBase::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & temp_waypoint.w[0] = vehicle_state_.position[0]; temp_waypoint.w[1] = vehicle_state_.position[1]; - temp_waypoint.w[2] = vehicle_state_.position[2]; + + if (vehicle_state_.position[2] < -default_altitude) { + + temp_waypoint.w[2] = vehicle_state_.position[2]; + } + else { + temp_waypoint.w[2] = -default_altitude; + } temp_waypoint.chi_d = 0.0; // Doesn't matter, it is never used. temp_waypoint.use_chi = false; diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp index 8233513..d8a3a5b 100644 --- a/rosplane/src/path_manager_example.cpp +++ b/rosplane/src/path_manager_example.cpp @@ -38,6 +38,12 @@ void PathManagerExample::manage(const Input & input, Output & output) RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "No waypoints received, orbiting origin at " << default_altitude << " meters."); output.flag = false; // Indicate that the path is an orbit. output.va_d = default_airspeed; // Set to the default_airspeed. + output.q[0] = 0.0f; // initialize the parameters to have a value. + output.q[1] = 0.0f; + output.q[2] = 0.0f; + output.r[0] = 0.0f; // initialize the parameters to have a value. + output.r[1] = 0.0f; + output.r[2] = 0.0f; output.c[0] = 0.0f; // Direcct the center of the orbit to the origin at the default default_altitude. output.c[1] = 0.0f; output.c[2] = -default_altitude; @@ -50,6 +56,12 @@ void PathManagerExample::manage(const Input & input, Output & output) // If only a single waypoint is given, orbit it. output.flag = false; output.va_d = waypoints_[0].va_d; + output.q[0] = 0.0f; // initialize the parameters to have a value. + output.q[1] = 0.0f; + output.q[2] = 0.0f; + output.r[0] = 0.0f; // initialize the parameters to have a value. + output.r[1] = 0.0f; + output.r[2] = 0.0f; output.c[0] = waypoints_[0].w[0]; output.c[1] = waypoints_[0].w[1]; output.c[2] = waypoints_[0].w[2]; @@ -143,6 +155,7 @@ void PathManagerExample::manage_fillet(const Input & input, if (orbit_last && idx_a_ == num_waypoints_ - 1) { + // TODO: check to see if this is the correct behavior. return; } @@ -561,8 +574,6 @@ void PathManagerExample::dubins_parameters(const Waypoint start_node, const Wayp void PathManagerExample::declare_parameters() { params_.declare_bool("orbit_last", false); - params_.declare_double("default_altitude", 50.0); - params_.declare_double("default_airspeed", 15.0); } int PathManagerExample::orbit_direction(float pn, float pe, float chi, float c_n, float c_e) diff --git a/rosplane_tuning/CMakeLists.txt b/rosplane_tuning/CMakeLists.txt index 8d9020e..1915ef4 100644 --- a/rosplane_tuning/CMakeLists.txt +++ b/rosplane_tuning/CMakeLists.txt @@ -57,29 +57,6 @@ install(TARGETS #### 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 diff --git a/rosplane_tuning/launch/rosplane_tuning_gui.launch.py b/rosplane_tuning/launch/rosplane_tuning_gui.launch.py deleted file mode 100644 index 3f75d0b..0000000 --- a/rosplane_tuning/launch/rosplane_tuning_gui.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -import os -import sys -from launch import LaunchDescription -from launch.descriptions import executable -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - # Create the package directory - rosplane_dir = get_package_share_directory('rosplane') - - return LaunchDescription([ - Node( - package='rosplane_tuning', - executable='tuning_gui.py', - name='tuning_gui_node', - output = 'screen' - ) - ]) diff --git a/rosplane_tuning/package.xml b/rosplane_tuning/package.xml index 0d1503a..ed245f2 100644 --- a/rosplane_tuning/package.xml +++ b/rosplane_tuning/package.xml @@ -23,7 +23,6 @@ ament_cmake - diff --git a/rosplane_tuning/plugin.xml b/rosplane_tuning/plugin.xml deleted file mode 100644 index 38dd565..0000000 --- a/rosplane_tuning/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - Tuning GUI for ROSflight - - - - - - - system-help - Super helpful GUI for tuning ROSplane. - - - diff --git a/rosplane_tuning/resource/tuning_gui.ui b/rosplane_tuning/resource/tuning_gui.ui deleted file mode 100644 index 120639b..0000000 --- a/rosplane_tuning/resource/tuning_gui.ui +++ /dev/null @@ -1,869 +0,0 @@ - - - ROSPlaneTuningGUI - - - - 0 - 0 - 995 - 856 - - - - - 0 - 0 - - - - - 810 - 610 - - - - MainWindow - - - - - 20 - 20 - 810 - 631 - - - - - 0 - 0 - - - - - 800 - 600 - - - - - 16777215 - 16777215 - - - - - - -1 - -1 - 811 - 631 - - - - - - - Qt::Horizontal - - - QSizePolicy::Maximum - - - - 10 - 20 - - - - - - - - QFrame::Plain - - - 2 - - - Qt::Horizontal - - - - - - - - - - 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::SetFixedSize - - - - - 6 - - - QLayout::SetDefaultConstraint - - - 0 - - - - - Qt::Horizontal - - - QSizePolicy::Maximum - - - - 10 - 20 - - - - - - - - 0 - - - QLayout::SetDefaultConstraint - - - 10 - - - 10 - - - - - - 16777215 - 40 - - - - Mode Select - - - - - - - - 16777215 - 10 - - - - QFrame::Plain - - - 1 - - - Qt::Horizontal - - - - - - - Course - - - - - - - Roll - - - - - - - Pitch - - - - - - - Airspeed - - - - - - - Altitude - - - - - - - Run - - - - - - - Undo - - - - - - - - 0 - 0 - - - - - 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 - - - - - 16777215 - 40 - - - - Qt::Horizontal - - - QSlider::TicksBelow - - - 0 - - - - - - - Qt::Vertical - - - QSizePolicy::Maximum - - - - 20 - 100 - - - - - - - - - - - - - - - 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 - 100 - - - - - - - - - - - - - - - 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 - 100 - - - - - - - - - - - - - - - - 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/scripts/rqt_tuning_gui b/rosplane_tuning/scripts/rqt_tuning_gui deleted file mode 100644 index 4987816..0000000 --- a/rosplane_tuning/scripts/rqt_tuning_gui +++ /dev/null @@ -1,9 +0,0 @@ -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 deleted file mode 100644 index e69de29..0000000 diff --git a/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py b/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py deleted file mode 100644 index 36985e5..0000000 --- a/rosplane_tuning/src/rqt_tuning_gui/rosflight_tuning_gui.py +++ /dev/null @@ -1,531 +0,0 @@ -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 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 - 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 = 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') - # 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) - - # 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 - 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 - - 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