diff --git a/data_viz/CHANGELOG.rst b/data_viz/CHANGELOG.rst deleted file mode 100644 index 3f738fa..0000000 --- a/data_viz/CHANGELOG.rst +++ /dev/null @@ -1,8 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package data_viz -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.0.0 (2023-08-11) ------------------- -* TODO: Put changelog here -* Contributors: diff --git a/data_viz/data_viz/__init__.py b/data_viz/data_viz/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/data_viz/package.xml b/data_viz/package.xml deleted file mode 100644 index 2c6f422..0000000 --- a/data_viz/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - data_viz - 1.0.0 - TODO: Package description - ian - BSD - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - rclpy - - - ament_python - - diff --git a/data_viz/resource/data_viz b/data_viz/resource/data_viz deleted file mode 100644 index e69de29..0000000 diff --git a/data_viz/setup.cfg b/data_viz/setup.cfg deleted file mode 100644 index 1d68caf..0000000 --- a/data_viz/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/data_viz -[install] -install_scripts=$base/lib/data_viz diff --git a/data_viz/setup.py b/data_viz/setup.py deleted file mode 100644 index 861b9f6..0000000 --- a/data_viz/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import setup - -package_name = 'data_viz' - -setup( - name=package_name, - version='0.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='ian', - maintainer_email='ian.young.reid@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'viz_data = data_viz.data_visualization_node:main' - ], - }, -) diff --git a/data_viz/test/test_copyright.py b/data_viz/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/data_viz/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/data_viz/test/test_flake8.py b/data_viz/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/data_viz/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/data_viz/test/test_pep257.py b/data_viz/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/data_viz/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/rosplane/CMakeLists.txt b/rosplane/CMakeLists.txt index 0970d6a..5428797 100644 --- a/rosplane/CMakeLists.txt +++ b/rosplane/CMakeLists.txt @@ -93,15 +93,6 @@ install(TARGETS rosplane_estimator_node DESTINATION lib/${PROJECT_NAME}) -# Signal Generator -add_executable(tuning_signal_generator - src/tuning_signal_generator.cpp) -ament_target_dependencies(tuning_signal_generator rosplane_msgs std_srvs rclcpp) -target_compile_options(tuning_signal_generator PRIVATE -Wno-unused-parameter) -install(TARGETS - tuning_signal_generator - DESTINATION lib/${PROJECT_NAME}) - #### END OF EXECUTABLES ### diff --git a/rosplane_tuning/CHANGELOG.rst b/rosplane_tuning/CHANGELOG.rst new file mode 100644 index 0000000..09c0551 --- /dev/null +++ b/rosplane_tuning/CHANGELOG.rst @@ -0,0 +1,6 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rosplane +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (Pre-release) +------------------ diff --git a/rosplane_tuning/CMakeLists.txt b/rosplane_tuning/CMakeLists.txt new file mode 100644 index 0000000..631836a --- /dev/null +++ b/rosplane_tuning/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(rosplane_tuning) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release") +endif(NOT CMAKE_BUILD_TYPE) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosplane_msgs REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) +find_package(rosflight_msgs REQUIRED) + +ament_export_dependencies(rclcpp rclpy) +ament_export_include_directories(include) + +#install(DIRECTORY originalFiles DESTINATION share/${PROJECT_NAME}/) +include_directories( #use this if you need .h files for include statements. The include will need to have the directories where each .h is respectively. + include + ${EIGEN3_INCLUDE_DIRS} +) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) + +### START OF REAL EXECUTABLES ### + +# Signal Generator +add_executable(signal_generator + src/signal_generator.cpp) +ament_target_dependencies(signal_generator rosplane_msgs std_srvs rclcpp) +target_compile_options(signal_generator PRIVATE -Wno-unused-parameter) +install(TARGETS + signal_generator + DESTINATION lib/${PROJECT_NAME}) + +# Data viz +install(PROGRAMS + src/data_viz/data_storage.py + src/data_viz/data_viz.py + src/data_viz/plot_window.py + DESTINATION lib/${PROJECT_NAME} +) + +#### END OF EXECUTABLES ### + +ament_package() diff --git a/rosplane_tuning/README.md b/rosplane_tuning/README.md new file mode 100644 index 0000000..0a4703e --- /dev/null +++ b/rosplane_tuning/README.md @@ -0,0 +1,65 @@ +# ROSplane Tuning + +This ROS2 package contains tools useful for tuning the autopilot performance of ROSplane. + +We recommend using [PlotJuggler](https://github.com/facontidavide/PlotJuggler) to visualize command input and response. + +## Signal Generator + +Signal generator is a ROS2 node that will generate step inputs, square waves, sine waves, sawtooth waves, and triangle waves to be used as command input for ROSplane. It has support for roll, pitch, altitude, heading, and airspeed command input. + +This is useful for tuning autopilots as we can give a clear and repeatable command to any portion of the autopilot and observe its response to that input. We can then tune gains, re-issue the same commands, and observe whether performance improved or worsened. + +Signal generator works by publishing autopilot commands on the ROS network on the `controller_commands` and `tuning_debug` topics. Each control output (roll, pitch, altitude, heading, airspeed) has a default values that is set by a ROS parameter. The signal generator will then add the generated signal to one of the control outputs with a set magnitude and frequency. + +### Signal Types +- Step: This is a non-continuous signal, where the generated signal will jump between the default value and the default+magnitude every time the `toggle_step_signal` service is called. +- Sine: This is a continuous signal that create a standard sine wave. +- Square: This is a continuous signal that creates a square type pattern. +- Triangle: This is a continuous signal that ramps up and down between the minimum and maximum value of the signal, creating a triangle like pattern. +- Sawtooth: This is a continuous signal (sometimes call a ramp signal) that creates a constantly increasing signal that resets to its minimum value once the maximum value is reached. + +![Waveforms](Waveforms.svg) + +*By Omegatron - Own work, CC BY-SA 3.0, https://commons.wikimedia.org/w/index.php?curid=343520* + +### Parameters +- `controller_output`: Specifies what controller to apply the generated signal to. All other controllers will be constant at their default values. Valid values are `roll`, `pitch`, `altitude`, `heading`, and `airspeed`. +- `signal_type`: Specified what kind of signal to generate. Valid values are `step`, `square`, `sawtooth`, `triangle`, and `sine`. +- `publish_rate_hz`: Specifies the rate to publish control commands. Must be greater than 0. +- `signal_magnitude`: Specifies the magnitude of the signal to generate. The signal will only be added to the default value, rather than subtracted. For example, if the signal has a magnitude of 2 and a default value of 5, the generated signal will range from 5 to 7. +- `frequency_hz`: Specifies the frequency of the generated signal. Must be greater than 0, and does not apply to step signals. For step signals, manually toggle the signal up and down with the `toggle_step_signal` service. +- `default_va_c`: The default value for the commanded airspeed, in meters per second. +- `default_h_c`: The default value for the commanded altitude, in meters above takeoff altitude. +- `default_chi_c`: The default value for the commanded heading, in radians clockwise from north. +- `default_theta_c`: The default value for the commanded pitch, in radians pitched up from horizontal. +- `default_phi_c`: The default value for the commanded roll, in radians 'rolled right' from horizontal. + +To get a parameter from the command line, use this command, replacing with the desired parameter to get. +``` +ros2 param get signal_generator +``` + +To set a parameter from the command line, use this command, replacing with the name of the parameter to set. Enter number parameters as float values, not integers (i.e. 1.0, not 1). +``` +ros2 param set signal_generator +``` + +### ROS Services +- `toggle_step_signal`: Toggles the step signal up and down. Does not apply to any other type of signal. +- `reset_signal`: Stops the generated signal and sets it to its default value. +- `pause_signal`: Pauses the generated signal at its current value. Does not apply to step signal. +- `start_continuous_signal`: Starts the signal generator at its current value, running continously until manually stopped. Does not apply to step signal. +- `start_single_period_signal`: Starts the signal generator at its current value, stopping after one full cycle. Does not apply to step signal. + +To call a service from the command line use this command, replacing with the name of the service you wish to call. +``` +ros2 service call std_srvs/srv/Trigger +``` + +## Data Viz + +Data viz is a python utility that can be used to plot the controller commands and response, as an alternative to PlotJuggler. To run data viz, +``` +ros2 run rosplane_tuning data_viz.py +``` diff --git a/rosplane_tuning/Waveforms.svg b/rosplane_tuning/Waveforms.svg new file mode 100644 index 0000000..0771ca9 --- /dev/null +++ b/rosplane_tuning/Waveforms.svg @@ -0,0 +1,82 @@ + + + + Sinus + Sine + Seno + Sinusoid + Sinusoïdale + Sinus + Sinus + синусен + Sinusoidalna + Синусоида + Sinus + Sinüs + Синусоїда + +SinusoidalSine + + Rechteck + Square + Cuadrada + Nelinurklaine + Carrée + Persegi + Pasagi + квадратен + Prostokątna + Меандр + Fyrkant + Kare + Прямокутник + +QuadradaSquare + + Dreieck + Triangle + Triangular + Kolmnurk-laine + Triangulaire + Segitiga + Maju-telu + триаголен + Trójkątna + Треугольная + Triangel + Üçgen + Трикутник + +TriangularTriangle + + Sägezahn + Sawtooth + Dientesde sierra + Saehammas-laine + Dents de scie + Gigi gergaji + Untu graji + назабен + Piłokształtna + Пила + Sågtand + Testere dişi + Пилкоподібна + +De serraSawtooth + + + + + + + + + + + + + + + + diff --git a/rosplane/include/tuning_signal_generator.hpp b/rosplane_tuning/include/signal_generator.hpp similarity index 100% rename from rosplane/include/tuning_signal_generator.hpp rename to rosplane_tuning/include/signal_generator.hpp diff --git a/rosplane/launch/rosplane_tuning.launch.py b/rosplane_tuning/launch/rosplane_tuning.launch.py similarity index 93% rename from rosplane/launch/rosplane_tuning.launch.py rename to rosplane_tuning/launch/rosplane_tuning.launch.py index a848012..8e9a1b8 100644 --- a/rosplane/launch/rosplane_tuning.launch.py +++ b/rosplane_tuning/launch/rosplane_tuning.launch.py @@ -16,10 +16,10 @@ def generate_launch_description(): for arg in sys.argv: if arg.startswith("control_type:="): control_type = arg.split(":=")[1] - + if arg.startswith("aircraft:="): aircraft = arg.split(":=")[1] - + autopilot_params = os.path.join( rosplane_dir, 'params', @@ -44,8 +44,8 @@ def generate_launch_description(): name='estimator' ), Node( - package='rosplane', - executable='tuning_signal_generator', + package='rosplane_tuning', + executable='signal_generator', name='signal_generator', output = 'screen' ) diff --git a/rosplane_tuning/package.xml b/rosplane_tuning/package.xml new file mode 100644 index 0000000..9323df2 --- /dev/null +++ b/rosplane_tuning/package.xml @@ -0,0 +1,29 @@ + + + + rosplane_tuning + 1.0.0 + TODO: Package description + controls + BSD + + ament_cmake + ament_cmake_python + + rclcpp + rclpy + std_msgs + std_srvs + sensor_msgs + rosplane_msgs + rosflight_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + + diff --git a/data_viz/data_viz/data_storage.py b/rosplane_tuning/src/data_viz/data_storage.py similarity index 100% rename from data_viz/data_viz/data_storage.py rename to rosplane_tuning/src/data_viz/data_storage.py diff --git a/data_viz/data_viz/data_visualization_node.py b/rosplane_tuning/src/data_viz/data_viz.py similarity index 98% rename from data_viz/data_viz/data_visualization_node.py rename to rosplane_tuning/src/data_viz/data_viz.py index 70d9217..d6695bb 100755 --- a/data_viz/data_viz/data_visualization_node.py +++ b/rosplane_tuning/src/data_viz/data_viz.py @@ -8,8 +8,8 @@ from rclpy.executors import SingleThreadedExecutor import time as pytime import threading -from data_viz.plot_window import PlotWindow, PlotNav, PlotAxis, PlotDiff -from data_viz.data_storage import RosStorageInterface +from plot_window import PlotWindow, PlotNav, PlotAxis, PlotDiff +from data_storage import RosStorageInterface class StatePlotter: """ Creates a plotting windows and repeatedly plots the data @@ -232,4 +232,4 @@ def main(args=None): thread.join() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/data_viz/data_viz/plot_window.py b/rosplane_tuning/src/data_viz/plot_window.py similarity index 100% rename from data_viz/data_viz/plot_window.py rename to rosplane_tuning/src/data_viz/plot_window.py diff --git a/rosplane/src/tuning_signal_generator.cpp b/rosplane_tuning/src/signal_generator.cpp similarity index 99% rename from rosplane/src/tuning_signal_generator.cpp rename to rosplane_tuning/src/signal_generator.cpp index c3f87c9..21d0b48 100644 --- a/rosplane/src/tuning_signal_generator.cpp +++ b/rosplane_tuning/src/signal_generator.cpp @@ -41,7 +41,7 @@ #include #include -#include "tuning_signal_generator.hpp" +#include "signal_generator.hpp" namespace rosplane {