diff --git a/.pylintrc b/.pylintrc new file mode 100644 index 00000000..a5208fdc --- /dev/null +++ b/.pylintrc @@ -0,0 +1,3 @@ +[main] +disable=duplicate-code,fixme +extension-pkg-whitelist=PyQt6,cv2 \ No newline at end of file diff --git a/README.md b/README.md index 4c1bb6bf..475afc8a 100644 --- a/README.md +++ b/README.md @@ -29,7 +29,7 @@ git clone --recurse-submodules git@github.com:cwruRobotics/rov-24.git If you've never contributed to a git repository before, you might receive an error message saying you don't have access. In that case visit [this tutorial](https://docs.github.com/en/authentication/connecting-to-github-with-ssh/about-ssh) to set up SSH for local GitHub access. -After cloning the code, we need to set up our IDE: VSCode. If you already have it, great. Otherwise follow [this](https://code.visualstudio.com/download) tutorial. +After cloning the code, we need to set up our IDE: VSCode. If you already have it, great. Otherwise follow [this](https://code.visualstudio.com/download) tutorial. We recommend installing the mypy, flake8 and autoDocstring VSCode extensions. Our setting for autoDocstring are set to Numpy and auto docstring on new line. ### Linux @@ -134,7 +134,9 @@ If you're working on package's `setup.py` or rov_msgs, you'll need to run `🏃 If you want to run our unit tests use this command `colcon test --event-handlers=console_direct+`. -It runs the tests and pipes the output to the terminal. +In VSCode, press `F1` or `ctrl+shift+p` and enter `Tasks: Run Test Task` as another method to run the above command. + +It runs the tests and pipes the output to the terminal. To test pi_main make sure to type your password into the terminal after running the above command. If you install the flake8 and mypy extension they should help enforce the linters. @@ -173,5 +175,26 @@ Any topics or services communicating across will be renamed first into the tethe Documentation will take place at 3 levels: - High Level - Overarching Design Document outlining our general structure and what goes where. -- Device Level - ROS Docs as set out in the ROS2 standards. -- Inline Level - Inline Documentation to the level that someone who has some basic code knowledge can understand what the code does. +- Device Level - Following the markdown tempate in `doc` directory. +- Inline Level - Using reST / Numpy Standard. To autogenerate in VSCode we use autoDocstring extension with the setting set to Numpy and auto docstring on new line. Below is an example of an inline function docstring. + +```python +def __init__(self, srv_type: SrvType, topic: str, signal: pyqtBoundSignal, + timeout: float = 1.0, expected_namespace: str = '/surface/gui'): + """ + _summary_ + + Parameters + ---------- + srv_type : SrvType + _description_ + topic : str + _description_ + signal : pyqtBoundSignal + _description_ + timeout : float, optional + _description_, by default 1.0 + expected_namespace : str, optional + _description_, by default '/surface/gui' + """ +``` diff --git a/src/pi/camera_streamer/launch/camera_launch.py b/src/pi/camera_streamer/launch/camera_launch.py index 6bdafa40..be904c14 100644 --- a/src/pi/camera_streamer/launch/camera_launch.py +++ b/src/pi/camera_streamer/launch/camera_launch.py @@ -1,9 +1,18 @@ +"""camera_streamer launch file.""" from launch.launch_description import LaunchDescription from launch_ros.actions import Node def generate_launch_description() -> LaunchDescription: + """ + Generate LaunchDescription for camera_streamer. + Returns + ------- + LaunchDescription + Launches Front and Bottom Cameras nodes + + """ # Symlinks are auto-generated by V4L2 # ls /dev/v4l/by-id to see all the available symlinks # ls /dev/v4l/by-path for usb slot based symlinks diff --git a/src/pi/camera_streamer/setup.py b/src/pi/camera_streamer/setup.py index 4de5f730..1dcab2c8 100644 --- a/src/pi/camera_streamer/setup.py +++ b/src/pi/camera_streamer/setup.py @@ -1,20 +1,21 @@ +"""setup.py for camera_streamer module.""" import os from glob import glob from setuptools import setup -package_name = 'camera_streamer' +PACKAGE_NAME = 'camera_streamer' setup( - name=package_name, + name=PACKAGE_NAME, version='1.0.0', - packages=[package_name], + packages=[PACKAGE_NAME], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.[pxy][yma]*')) ], install_requires=['setuptools', 'flake8==4.0.1', 'mypy >= 1.7'], diff --git a/src/pi/camera_streamer/test/test_flake8.py b/src/pi/camera_streamer/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/pi/camera_streamer/test/test_flake8.py +++ b/src/pi/camera_streamer/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/pi/camera_streamer/test/test_mypy.py b/src/pi/camera_streamer/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/pi/camera_streamer/test/test_mypy.py +++ b/src/pi/camera_streamer/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/pi/camera_streamer/test/test_pep257.py b/src/pi/camera_streamer/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/pi/camera_streamer/test/test_pep257.py +++ b/src/pi/camera_streamer/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/pi/manipulators/setup.py b/src/pi/manipulators/setup.py index b6122ceb..6c45869d 100644 --- a/src/pi/manipulators/setup.py +++ b/src/pi/manipulators/setup.py @@ -1,24 +1,21 @@ +"""setup.py for manipulators module.""" import os -import sys from glob import glob from setuptools import setup -major_num = sys.version_info[0] -minor_num = sys.version_info[1] - -package_name = 'manipulators' +PACKAGE_NAME = 'manipulators' setup( - name=package_name, + name=PACKAGE_NAME, version='1.0.0', - packages=[package_name], + packages=[PACKAGE_NAME], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.[pxy][yma]*')) ], install_requires=['setuptools', 'flake8==4.0.1', 'mypy >= 1.7'], diff --git a/src/pi/manipulators/test/test_flake8.py b/src/pi/manipulators/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/pi/manipulators/test/test_flake8.py +++ b/src/pi/manipulators/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/pi/manipulators/test/test_mypy.py b/src/pi/manipulators/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/pi/manipulators/test/test_mypy.py +++ b/src/pi/manipulators/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/pi/manipulators/test/test_pep257.py b/src/pi/manipulators/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/pi/manipulators/test/test_pep257.py +++ b/src/pi/manipulators/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/pi/pi_main/launch/pi_launch.py b/src/pi/pi_main/launch/pi_launch.py index a1e5ce48..f5283446 100644 --- a/src/pi/pi_main/launch/pi_launch.py +++ b/src/pi/pi_main/launch/pi_launch.py @@ -1,3 +1,4 @@ +"""pi_launch launch file.""" import os from ament_index_python.packages import get_package_share_directory @@ -8,7 +9,16 @@ def generate_launch_description() -> LaunchDescription: - NS = 'pi' + """ + Generate LaunchDescription for pi_main. + + Returns + ------- + LaunchDescription + Launches camera_streamer package and pixhawk_communication package. + + """ + NAMESPACE = 'pi' # Manipulator Controller # manip_path: str = get_package_share_directory('manipulators') # @@ -56,7 +66,7 @@ def generate_launch_description() -> LaunchDescription: namespace_launch = GroupAction( actions=[ - PushRosNamespace(NS), + PushRosNamespace(NAMESPACE), # manip_launch, pixhawk_launch, cam_launch, diff --git a/src/pi/pi_main/pi_main/install_on_boot.py b/src/pi/pi_main/pi_main/run_on_boot.py similarity index 73% rename from src/pi/pi_main/pi_main/install_on_boot.py rename to src/pi/pi_main/pi_main/run_on_boot.py index 48808544..ea7f29ad 100644 --- a/src/pi/pi_main/pi_main/install_on_boot.py +++ b/src/pi/pi_main/pi_main/run_on_boot.py @@ -1,3 +1,4 @@ +"""When run sets up environment for the robot to run on boot.""" import os import sys import subprocess @@ -8,6 +9,13 @@ def main() -> None: + """ + Set up Pi file environment. + + Copies udev rules from this package into udev folder. + Also uses robot_upstart to allow robot to automatically start on power on. + + """ pi_main_share = get_package_share_directory('pi_main') launch_dir = os.path.join(pi_main_share, 'launch') @@ -27,14 +35,14 @@ def main() -> None: cmd = ['/usr/bin/sudo', '/usr/bin/python3', udev_script, pi_main_share] try: - p = subprocess.run(cmd, capture_output=True, check=True) + process = subprocess.run(cmd, capture_output=True, check=True) # Logs Error - except subprocess.CalledProcessError as e: - print(e.stderr) + except subprocess.CalledProcessError as error: + print(error.stderr) sys.exit(1) # Success Message - print(p.stdout.decode()) + print(process.stdout.decode()) install_path = os.path.join(pi_main_share, "..", "..") workspace_path = os.path.join(install_path, "setup.bash") diff --git a/src/pi/pi_main/pi_main/udev_copy.py b/src/pi/pi_main/pi_main/udev_copy.py index 539eaac4..1ee98548 100644 --- a/src/pi/pi_main/pi_main/udev_copy.py +++ b/src/pi/pi_main/pi_main/udev_copy.py @@ -1,3 +1,4 @@ +"""Copies udev rules from separate process to ensure ideal protections of sudo.""" import os import shutil import sys diff --git a/src/pi/pi_main/setup.py b/src/pi/pi_main/setup.py index 1cbc9564..6e345e61 100644 --- a/src/pi/pi_main/setup.py +++ b/src/pi/pi_main/setup.py @@ -1,23 +1,24 @@ +"""setup.py for pi_main module.""" import os from glob import glob from setuptools import setup -package_name = 'pi_main' +PACKAGE_NAME = 'pi_main' setup( - name=package_name, + name=PACKAGE_NAME, version='1.0.0', - packages=[package_name], + packages=[PACKAGE_NAME], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.[pxy][yma]*')), - (os.path.join('share', package_name, 'udev_rules'), + (os.path.join('share', PACKAGE_NAME, 'udev_rules'), glob('udev_rules/*')) ], install_requires=['setuptools', 'flake8==4.0.1', 'mypy >= 1.7'], @@ -29,7 +30,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'install = pi_main.install_on_boot:main', + 'install = pi_main.run_on_boot:main', ], }, ) diff --git a/src/pi/pi_main/test/test_flake8.py b/src/pi/pi_main/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/pi/pi_main/test/test_flake8.py +++ b/src/pi/pi_main/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/pi/pi_main/test/test_mypy.py b/src/pi/pi_main/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/pi/pi_main/test/test_mypy.py +++ b/src/pi/pi_main/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/pi/pi_main/test/test_pep257.py b/src/pi/pi_main/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/pi/pi_main/test/test_pep257.py +++ b/src/pi/pi_main/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/pi/pi_main/test/test_install_on_boot.py b/src/pi/pi_main/test/test_run_on_boot.py similarity index 95% rename from src/pi/pi_main/test/test_install_on_boot.py rename to src/pi/pi_main/test/test_run_on_boot.py index c37ffa54..478314dd 100644 --- a/src/pi/pi_main/test/test_install_on_boot.py +++ b/src/pi/pi_main/test/test_run_on_boot.py @@ -1,6 +1,6 @@ import os -from pi_main.install_on_boot import main +from pi_main.run_on_boot import main ROS_DISTRO = os.getenv("ROS_DISTRO") diff --git a/src/pi/pixhawk_communication/launch/mavros_launch.py b/src/pi/pixhawk_communication/launch/mavros_launch.py index 796cf701..049654cc 100644 --- a/src/pi/pixhawk_communication/launch/mavros_launch.py +++ b/src/pi/pixhawk_communication/launch/mavros_launch.py @@ -1,9 +1,18 @@ +"""mavros_launch launch file.""" from launch.launch_description import LaunchDescription from launch_ros.actions import Node def generate_launch_description() -> LaunchDescription: + """ + Generate LaunchDescription for pixhawk_communication package. + Returns + ------- + LaunchDescription + Launches mavros Node. + + """ mavros_node = Node( package="mavros", executable="mavros_node", diff --git a/src/pi/pixhawk_communication/setup.py b/src/pi/pixhawk_communication/setup.py index d38c15a0..f55542a6 100644 --- a/src/pi/pixhawk_communication/setup.py +++ b/src/pi/pixhawk_communication/setup.py @@ -1,20 +1,21 @@ +"""setup.py for pixhawk_communication module.""" import os from glob import glob from setuptools import setup -package_name = 'pixhawk_communication' +PACKAGE_NAME = 'pixhawk_communication' setup( - name=package_name, + name=PACKAGE_NAME, version='1.0.0', - packages=[package_name], + packages=[PACKAGE_NAME], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.[pxy][yma]*')) ], install_requires=['setuptools', 'flake8==4.0.1', 'mypy >= 1.7'], diff --git a/src/pi/pixhawk_communication/test/test_flake8.py b/src/pi/pixhawk_communication/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/pi/pixhawk_communication/test/test_flake8.py +++ b/src/pi/pixhawk_communication/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/pi/pixhawk_communication/test/test_mypy.py b/src/pi/pixhawk_communication/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/pi/pixhawk_communication/test/test_mypy.py +++ b/src/pi/pixhawk_communication/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/pi/pixhawk_communication/test/test_pep257.py b/src/pi/pixhawk_communication/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/pi/pixhawk_communication/test/test_pep257.py +++ b/src/pi/pixhawk_communication/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/pi/realsense/launch/realsense_launch.py b/src/pi/realsense/launch/realsense_launch.py index 299b320f..b998f2a0 100644 --- a/src/pi/realsense/launch/realsense_launch.py +++ b/src/pi/realsense/launch/realsense_launch.py @@ -1,3 +1,4 @@ +"""realsense_launch launch file.""" import os from ament_index_python.packages import get_package_share_directory @@ -8,7 +9,15 @@ def generate_launch_description() -> LaunchDescription: + """ + Generate LaunchDescription for the realsense package. + Returns + ------- + LaunchDescription + Launches realsense launch file. + + """ realsense_path: str = get_package_share_directory('realsense2_camera') # Launches Realsense diff --git a/src/pi/realsense/setup.py b/src/pi/realsense/setup.py index beaf63ef..05114fa5 100644 --- a/src/pi/realsense/setup.py +++ b/src/pi/realsense/setup.py @@ -1,21 +1,22 @@ +"""setup.py for the realsense module.""" import os from glob import glob from setuptools import setup -package_name = 'realsense' +PACKAGE_NAME = 'realsense' setup( - name=package_name, + name=PACKAGE_NAME, version='1.0.0', - packages=[package_name], + packages=[PACKAGE_NAME], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.[pxy][yma]*')) ], install_requires=['setuptools', 'flake8==4.0.1', 'mypy >= 1.7'], diff --git a/src/pi/realsense/test/test_flake8.py b/src/pi/realsense/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/pi/realsense/test/test_flake8.py +++ b/src/pi/realsense/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/pi/realsense/test/test_mypy.py b/src/pi/realsense/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/pi/realsense/test/test_mypy.py +++ b/src/pi/realsense/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/pi/realsense/test/test_pep257.py b/src/pi/realsense/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/pi/realsense/test/test_pep257.py +++ b/src/pi/realsense/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/rov_msgs/CMakeLists.txt b/src/rov_msgs/CMakeLists.txt index 64407199..1d005b64 100644 --- a/src/rov_msgs/CMakeLists.txt +++ b/src/rov_msgs/CMakeLists.txt @@ -26,10 +26,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "srv/TaskRequest.srv" - "action/Example.action" - "action/BasicTask.action" - "msg/TaskFeedback.msg" + "srv/AutonomousFlight.srv" "msg/Manip.msg" "msg/CameraControllerSwitch.msg" DEPENDENCIES std_msgs diff --git a/src/rov_msgs/README.md b/src/rov_msgs/README.md index 3deb881e..3671ff3b 100644 --- a/src/rov_msgs/README.md +++ b/src/rov_msgs/README.md @@ -14,22 +14,8 @@ Used for toggling through camera feeds in the pilot gui. Used for toggling manipulators on and off. -[TaskFeedback.msg](msg/TaskFeedback.msg) - -Used for communicating current task. - ## Srv files -[TaskRequest.srv](srv/TaskRequest.srv) - -Used for communicating current task and receiving response. - -## Action files - -[BasicTask.action](action/BasicTask.action) - -BasicTask responds with a string. - -[Example.action](action/Example.action) +[AutonomousFlight.srv](srv/AutonomousFlight.srv) -An example action. +Send to a automation task's control topic to start or stop that automation task diff --git a/src/rov_msgs/action/BasicTask.action b/src/rov_msgs/action/BasicTask.action deleted file mode 100644 index bf692017..00000000 --- a/src/rov_msgs/action/BasicTask.action +++ /dev/null @@ -1,3 +0,0 @@ ---- ---- -string feedback_message \ No newline at end of file diff --git a/src/rov_msgs/action/Example.action b/src/rov_msgs/action/Example.action deleted file mode 100644 index 1d1fffa5..00000000 --- a/src/rov_msgs/action/Example.action +++ /dev/null @@ -1,6 +0,0 @@ -bool morning -bool cheery ---- -string message ---- -string feedback_message \ No newline at end of file diff --git a/src/rov_msgs/msg/TaskFeedback.msg b/src/rov_msgs/msg/TaskFeedback.msg deleted file mode 100644 index 8d0f9a6f..00000000 --- a/src/rov_msgs/msg/TaskFeedback.msg +++ /dev/null @@ -1 +0,0 @@ -int64 task_id \ No newline at end of file diff --git a/src/rov_msgs/srv/AutonomousFlight.srv b/src/rov_msgs/srv/AutonomousFlight.srv new file mode 100644 index 00000000..81b522af --- /dev/null +++ b/src/rov_msgs/srv/AutonomousFlight.srv @@ -0,0 +1,5 @@ +# True to start an automation task, false to stop +bool start +--- +# True if automation task is now running, false otherwise +bool is_running \ No newline at end of file diff --git a/src/rov_msgs/srv/TaskRequest.srv b/src/rov_msgs/srv/TaskRequest.srv deleted file mode 100644 index dae703e3..00000000 --- a/src/rov_msgs/srv/TaskRequest.srv +++ /dev/null @@ -1,3 +0,0 @@ -int64 task_id ---- -string response \ No newline at end of file diff --git a/src/surface/flight_control/README.md b/src/surface/flight_control/README.md new file mode 100644 index 00000000..0aa567ad --- /dev/null +++ b/src/surface/flight_control/README.md @@ -0,0 +1,76 @@ +# Flight Control + +## Overview + +This package includes keyboard, PS5 controller, and automatic docking pilot nodes. It abstracts creating motor control messages with `PixhawkInstruction`s. + +## Usage + +The PS5 controller (`manual_control_node`) and the auto docking controller (`auto_docking_node`) are both run when the pilot is launched. +You can run them on their own with: + +```bash +ros2 launch flight_control flight_control_launch.py +``` + +The keyboard controller (`keyboard_control_node`) is not run normally. +You can run it with: + +```bash +ros2 launch flight_control keyboard_control_launch.py +``` + +## Launch files + +* **flight_control_launch.py:** launches the PS5 controller and readies the auto docking controller +* **keyboard_control_launch.py:** launches the keyboard controller under the `/surface` namespace + +## Nodes + +### manual_control_node + +Controls motors, manipulators, and camera switching (if applicable) from the PS5 controller. + +#### Subscribed Topics + +* **`/surface/joy`** ([sensor_msgs/msg/Joy]) + + PS5 controller instructions. + +#### Published Topics + +* **`/tether/mavros/rc/override`** ([mavros_msgs/msg/OverrideRcIn]) + + The movement instructions for the Pixhawk. + +* **`/tether/manipulator_control`** ([rov_msgs/msg/Manip]) + + Manipulator instructions for the Pi. + +* **`/surface/camera_switch`** ([rov_msgs/msg/CameraControllerSwitch]) + + Instructions to change which camera should be active. TODO: Remove this if possible after upgrading to FLIR cams. + +### keyboard_control_node + +Controls motors (only) from the keyboard. Not run by general surface launch files. Run this separately with its launch file to use it. +This node can publish concurrently with manual control/auto docking. + +#### Published Topics + +* **`/tether/mavros/rc/override`** ([mavros_msgs/msg/OverrideRcIn]) + + The movement instructions for the Pixhawk. This node can publish concurrently with manual control. + +### auto_docking_node + +Execute an automatic docking sequence. This node must be "activated" with its service before it will publish movement instructions. +Once activated, it will publish concurrently with manual control/keyboard control nodes. + +TODO: actually implement autodocking. + +#### Services + +* **`/surface/auto_docker_control`** ([rov_msgs/srv/AutonomousFlight.srv]) + + Toggles whether the auto docker is running (sending instructions) and returns the new state. diff --git a/src/surface/task_selector/doc/images/SelectorNodeGraph.png b/src/surface/flight_control/doc/images/SelectorNodeGraph.png similarity index 100% rename from src/surface/task_selector/doc/images/SelectorNodeGraph.png rename to src/surface/flight_control/doc/images/SelectorNodeGraph.png diff --git a/src/surface/keyboard_driver/keyboard_driver/__init__.py b/src/surface/flight_control/flight_control/__init__.py similarity index 100% rename from src/surface/keyboard_driver/keyboard_driver/__init__.py rename to src/surface/flight_control/flight_control/__init__.py diff --git a/src/surface/flight_control/flight_control/auto_docking_node.py b/src/surface/flight_control/flight_control/auto_docking_node.py new file mode 100644 index 00000000..0f8465a9 --- /dev/null +++ b/src/surface/flight_control/flight_control/auto_docking_node.py @@ -0,0 +1,33 @@ +from rov_msgs.srv import AutonomousFlight + +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node + + +class AutoDocker(Node): + + def __init__(self) -> None: + super().__init__('auto_docker', + parameter_overrides=[]) + + self.control_server = self.create_service( + AutonomousFlight, 'auto_docker_control', self.task_control_callback) + + self.running = False + + # TODO: Add cam frame subscriber here to act as control loop for auto docking + + def task_control_callback(self, request: AutonomousFlight.Request, + response: AutonomousFlight.Response) -> AutonomousFlight.Response: + self.running = request.start + response.is_running = self.running + + return response + + +def main() -> None: + rclpy.init() + auto_docker = AutoDocker() + executor = MultiThreadedExecutor() + rclpy.spin(auto_docker, executor=executor) diff --git a/src/surface/keyboard_driver/keyboard_driver/keyboard_driver_node.py b/src/surface/flight_control/flight_control/keyboard_control_node.py similarity index 69% rename from src/surface/keyboard_driver/keyboard_driver/keyboard_driver_node.py rename to src/surface/flight_control/flight_control/keyboard_control_node.py index c2f6a508..0c58142a 100644 --- a/src/surface/keyboard_driver/keyboard_driver/keyboard_driver_node.py +++ b/src/surface/flight_control/flight_control/keyboard_control_node.py @@ -1,15 +1,13 @@ -from array import array from typing import Optional import rclpy.utilities from mavros_msgs.msg import OverrideRCIn +from rclpy.publisher import Publisher from pynput.keyboard import Key, KeyCode, Listener from rclpy.node import Node from rclpy.qos import qos_profile_system_default -from task_selector.manual_control_node import (FORWARD_CHANNEL, - LATERAL_CHANNEL, PITCH_CHANNEL, - ROLL_CHANNEL, THROTTLE_CHANNEL, - YAW_CHANNEL) + +from flight_control.pixhawk_instruction import PixhawkInstruction # key bindings FORWARD = "w" @@ -54,19 +52,16 @@ [p] = Show this help""" -# Range of values Pixhawk takes -# In microseconds -ZERO_SPEED: int = 1500 -RANGE_SPEED: int = 400 - - class KeyboardListenerNode(Node): def __init__(self) -> None: - super().__init__("keyboard_listener_node", parameter_overrides=[]) + super().__init__('keyboard_listener_node', parameter_overrides=[]) - self.pub_status = self.create_publisher( - OverrideRCIn, "/mavros/rc/override", qos_profile_system_default + self.rc_pub: Publisher = self.create_publisher( + OverrideRCIn, + 'mavros/rc/override', + qos_profile_system_default ) + self.get_logger().info(HELP_MSG) self.status = { FORWARD: False, @@ -130,19 +125,16 @@ def on_release(self, key: Optional[Key | KeyCode]) -> None: raise exception def pub_rov_control(self) -> None: - msg = OverrideRCIn() - - channels = array('B', [1500, 1500, 1500, 1500, 1500, 1500]) - channels[FORWARD_CHANNEL] += (self.status[FORWARD] - self.status[BACKWARD]) * 400 - channels[LATERAL_CHANNEL] += (self.status[LEFT] - self.status[RIGHT]) * 400 - channels[THROTTLE_CHANNEL] += (self.status[UP] - self.status[DOWN]) * 400 - channels[ROLL_CHANNEL] += (self.status[ROLL_LEFT] - self.status[ROLL_RIGHT]) * 400 - channels[PITCH_CHANNEL] += (self.status[PITCH_UP] - self.status[PITCH_DOWN]) * 400 - channels[YAW_CHANNEL] += (self.status[YAW_LEFT] - self.status[YAW_RIGHT]) * 400 - - msg.channels = channels + instruction = PixhawkInstruction( + pitch=self.status[PITCH_UP] - self.status[PITCH_DOWN], + roll=self.status[ROLL_LEFT] - self.status[ROLL_RIGHT], + vertical=self.status[UP] - self.status[DOWN], + forward=self.status[FORWARD] - self.status[BACKWARD], + lateral=self.status[LEFT] - self.status[RIGHT], + yaw=self.status[YAW_LEFT] - self.status[YAW_RIGHT] + ) - self.pub_status.publish(msg) + self.rc_pub.publish(instruction.to_override_rc_in()) def spin(self) -> None: with Listener( diff --git a/src/surface/task_selector/task_selector/manual_control_node.py b/src/surface/flight_control/flight_control/manual_control_node.py similarity index 61% rename from src/surface/task_selector/task_selector/manual_control_node.py rename to src/surface/flight_control/flight_control/manual_control_node.py index 3f34787d..ce2e4747 100644 --- a/src/surface/task_selector/task_selector/manual_control_node.py +++ b/src/surface/flight_control/flight_control/manual_control_node.py @@ -1,18 +1,18 @@ from collections.abc import MutableSequence import rclpy -from mavros_msgs.msg import OverrideRCIn -from rclpy.action.server import ActionServer, CancelResponse, ServerGoalHandle from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from rclpy.publisher import Publisher from rclpy.qos import qos_profile_sensor_data, qos_profile_system_default from rclpy.subscription import Subscription from sensor_msgs.msg import Joy +from mavros_msgs.msg import OverrideRCIn -from rov_msgs.action import BasicTask from rov_msgs.msg import CameraControllerSwitch, Manip +from flight_control.pixhawk_instruction import PixhawkInstruction + # Button meanings for PS5 Control might be different for others X_BUTTON: int = 0 # Manipulator 0 O_BUTTON: int = 1 # Manipulator 1 @@ -39,45 +39,18 @@ DPADHOR: int = 6 DPADVERT: int = 7 -# Brown out protection -SPEED_THROTTLE: float = 0.85 - -# Range of values Pixhawk takes -# In microseconds -ZERO_SPEED: int = 1500 -MAX_RANGE_SPEED: int = 400 -RANGE_SPEED: float = MAX_RANGE_SPEED*SPEED_THROTTLE - -# Channels for RC command -MAX_CHANNEL: int = 8 -MIN_CHANNEL: int = 1 - -PITCH_CHANNEL: int = 0 # Pitch -ROLL_CHANNEL: int = 1 # Roll -THROTTLE_CHANNEL: int = 2 # Z -LATERAL_CHANNEL: int = 3 # Y -FORWARD_CHANNEL: int = 4 # X -YAW_CHANNEL: int = 5 # Yaw - class ManualControlNode(Node): - _passing: bool = False - def __init__(self) -> None: super().__init__('manual_control_node', parameter_overrides=[]) - # TODO would Service make more sense then Actions? - self._action_server: ActionServer = ActionServer( - self, - BasicTask, - 'manual_control', - self.execute_callback - ) + self.rc_pub: Publisher = self.create_publisher( OverrideRCIn, 'mavros/rc/override', qos_profile_system_default ) + self.subscription: Subscription = self.create_subscription( Joy, 'joy', @@ -109,71 +82,32 @@ def __init__(self) -> None: self.seen_right_cam = False def controller_callback(self, msg: Joy) -> None: - if self._passing: - self.joystick_to_pixhawk(msg) - self.manip_callback(msg) - self.camera_toggle(msg) + self.joystick_to_pixhawk(msg) + self.manip_callback(msg) + self.camera_toggle(msg) def joystick_to_pixhawk(self, msg: Joy) -> None: - rc_msg = OverrideRCIn() - axes: MutableSequence[float] = msg.axes buttons: MutableSequence[int] = msg.buttons - # DPad Pitch - rc_msg.channels[PITCH_CHANNEL] = self.joystick_profiles(axes[DPADVERT]) - - # L1/R1 Buttons for Roll - rc_msg.channels[ROLL_CHANNEL] = self.joystick_profiles(buttons[R1] - buttons[L1]) - - # Right Joystick Z - rc_msg.channels[THROTTLE_CHANNEL] = self.joystick_profiles(axes[RJOYX]) - - # Left Joystick X - rc_msg.channels[FORWARD_CHANNEL] = self.joystick_profiles(axes[LJOYX]) - - # Left Joystick Y - rc_msg.channels[LATERAL_CHANNEL] = self.joystick_profiles(-axes[LJOYY]) - - # L2/R2 Buttons for Yaw - rc_msg.channels[YAW_CHANNEL] = self.joystick_profiles((axes[R2PRESS_PERCENT] - - axes[L2PRESS_PERCENT])/2) - - self.rc_pub.publish(rc_msg) - - # Used to create smoother adjustments - def joystick_profiles(self, val: float) -> int: - return int(RANGE_SPEED * val * abs(val)) + ZERO_SPEED - - def execute_callback(self, goal_handle: ServerGoalHandle) -> BasicTask.Result: - self.get_logger().info('Starting Manual Control') - - if goal_handle.is_cancel_requested: - self._passing = False - - goal_handle.canceled() - self.get_logger().info('Ending Manual Control') - return BasicTask.Result() - else: - self._passing = True + instruction = PixhawkInstruction( + pitch=axes[DPADVERT], # DPad + roll=buttons[R1] - buttons[L1], # L1/R1 buttons + vertical=axes[RJOYX], # Right Joystick Z + forward=axes[LJOYX], # Left Joystick X + lateral=-axes[LJOYY], # Left Joystick Y + yaw=(axes[R2PRESS_PERCENT] - axes[L2PRESS_PERCENT]) / 2 # L2/R2 buttons + ) - feedback_msg = BasicTask.Feedback() - feedback_msg.feedback_message = "Task is executing" - goal_handle.publish_feedback(feedback_msg) - goal_handle.succeed() - return BasicTask.Result() + # Smooth out adjustments + instruction.apply(lambda value: value * abs(value)) - # TODO jank? - def cancel_callback(self, goal_handle: ServerGoalHandle) -> CancelResponse: - self.get_logger().info('Received cancel request') - self._passing = False - return CancelResponse.ACCEPT + self.rc_pub.publish(instruction.to_override_rc_in()) def manip_callback(self, msg: Joy) -> None: buttons: MutableSequence[int] = msg.buttons for button_id, manip_button in self.manip_buttons.items(): - just_pressed: bool = False if buttons[button_id] == 1: diff --git a/src/surface/flight_control/flight_control/pixhawk_instruction.py b/src/surface/flight_control/flight_control/pixhawk_instruction.py new file mode 100644 index 00000000..381ca3c7 --- /dev/null +++ b/src/surface/flight_control/flight_control/pixhawk_instruction.py @@ -0,0 +1,63 @@ +from mavros_msgs.msg import OverrideRCIn +from dataclasses import dataclass +from typing import Callable + +# Brown out protection +SPEED_THROTTLE: float = 0.85 + +# Range of values Pixhawk takes +# In microseconds +ZERO_SPEED: int = 1500 +MAX_RANGE_SPEED: int = 400 +RANGE_SPEED: float = MAX_RANGE_SPEED * SPEED_THROTTLE + +# Channels for RC command +MAX_CHANNEL: int = 8 +MIN_CHANNEL: int = 1 + +FORWARD_CHANNEL: int = 4 # X +THROTTLE_CHANNEL: int = 2 # Z (vertical) +LATERAL_CHANNEL: int = 3 # Y (left & right) +PITCH_CHANNEL: int = 0 # Pitch +YAW_CHANNEL: int = 5 # Yaw +ROLL_CHANNEL: int = 1 # Roll + + +@dataclass +class PixhawkInstruction: + """ + Store movement instructions for the Pixhawk. + + Each instruction should be a float in the range [-1.0, 1.0]. + """ + + forward: float = 0 + vertical: float = 0 + lateral: float = 0 + pitch: float = 0 + yaw: float = 0 + roll: float = 0 + + def apply(self, function_to_apply: Callable[[float], float]) -> None: + """Apply a function to each dimension of this PixhawkInstruction.""" + self.forward = function_to_apply(self.forward) + self.vertical = function_to_apply(self.vertical) + self.lateral = function_to_apply(self.lateral) + self.pitch = function_to_apply(self.pitch) + self.yaw = function_to_apply(self.yaw) + self.roll = function_to_apply(self.roll) + + def to_override_rc_in(self) -> OverrideRCIn: + """Convert this PixhawkInstruction to an rc_msg with the appropriate channels array.""" + rc_msg = OverrideRCIn() + + self.apply(lambda value: int(RANGE_SPEED * value) + ZERO_SPEED) + + rc_msg.channels[FORWARD_CHANNEL] = self.forward + rc_msg.channels[THROTTLE_CHANNEL] = self.vertical + rc_msg.channels[LATERAL_CHANNEL] = self.lateral + rc_msg.channels[PITCH_CHANNEL] = self.pitch + rc_msg.channels[YAW_CHANNEL] = self.yaw + rc_msg.channels[ROLL_CHANNEL] = self.roll + + return rc_msg diff --git a/src/surface/flight_control/launch/flight_control_launch.py b/src/surface/flight_control/launch/flight_control_launch.py new file mode 100644 index 00000000..45ee13e2 --- /dev/null +++ b/src/surface/flight_control/launch/flight_control_launch.py @@ -0,0 +1,27 @@ +from launch.launch_description import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + manual_control_node: Node = Node( + package='flight_control', + executable='manual_control_node', + remappings=[('/surface/manipulator_control', '/tether/manipulator_control'), + ('/surface/mavros/rc/override', '/tether/mavros/rc/override')], + emulate_tty=True, + output='screen' + ) + + auto_docking_node: Node = Node( + package='flight_control', + executable='auto_docking_node', + remappings=[('/surface/manipulator_control', '/tether/manipulator_control'), + ('/surface/mavros/rc/override', '/tether/mavros/rc/override')], + emulate_tty=True, + output='screen' + ) + + return LaunchDescription([ + manual_control_node, + auto_docking_node + ]) diff --git a/src/surface/flight_control/launch/keyboard_control_launch.py b/src/surface/flight_control/launch/keyboard_control_launch.py new file mode 100644 index 00000000..ec479c7c --- /dev/null +++ b/src/surface/flight_control/launch/keyboard_control_launch.py @@ -0,0 +1,24 @@ +from launch.actions import GroupAction +from launch.launch_description import LaunchDescription +from launch_ros.actions import Node, PushRosNamespace + + +def generate_launch_description() -> LaunchDescription: + keyboard_control_node: Node = Node( + package='flight_control', + executable='keyboard_control_node', + remappings=[('/surface/mavros/rc/override', '/tether/mavros/rc/override')], + emulate_tty=True, + output='screen' + ) + + namespace_launch: GroupAction = GroupAction( + actions=[ + PushRosNamespace("surface"), + keyboard_control_node + ] + ) + + return LaunchDescription([ + namespace_launch + ]) diff --git a/src/surface/task_selector/package.xml b/src/surface/flight_control/package.xml similarity index 80% rename from src/surface/task_selector/package.xml rename to src/surface/flight_control/package.xml index 4d08bfbf..78689a5a 100644 --- a/src/surface/task_selector/package.xml +++ b/src/surface/flight_control/package.xml @@ -1,10 +1,10 @@ - task_selector + flight_control 1.0.0 - Mate ROV task scheduler and task nodes - ericy + Mate ROV sub movement controllers + Benjamin Apache License 2.0 rclpy diff --git a/src/surface/keyboard_driver/resource/keyboard_driver b/src/surface/flight_control/resource/flight_control similarity index 100% rename from src/surface/keyboard_driver/resource/keyboard_driver rename to src/surface/flight_control/resource/flight_control diff --git a/src/surface/flight_control/setup.cfg b/src/surface/flight_control/setup.cfg new file mode 100644 index 00000000..27fb759a --- /dev/null +++ b/src/surface/flight_control/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/flight_control +[install] +install_scripts=$base/lib/flight_control diff --git a/src/surface/flight_control/setup.py b/src/surface/flight_control/setup.py new file mode 100644 index 00000000..fecd198a --- /dev/null +++ b/src/surface/flight_control/setup.py @@ -0,0 +1,34 @@ +import os +from glob import glob + +from setuptools import setup + +package_name = 'flight_control' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + # Include all launch files. + (os.path.join('share', package_name, 'launch'), + glob('launch/*launch.[pxy][yma]*')) + ], + install_requires=['setuptools', 'flake8==4.0.1', 'mypy>=1.7', 'pynput'], + zip_safe=True, + maintainer='benjamin', + maintainer_email='bwp18@case.edu', + description='Mate ROV sub movement controllers', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'manual_control_node = flight_control.manual_control_node:main', + 'keyboard_control_node = flight_control.keyboard_control_node:main', + 'auto_docking_node = flight_control.auto_docking_node:main' + ], + }, +) diff --git a/src/surface/keyboard_driver/test/test_flake8.py b/src/surface/flight_control/test/test_flake8.py similarity index 77% rename from src/surface/keyboard_driver/test/test_flake8.py rename to src/surface/flight_control/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/surface/keyboard_driver/test/test_flake8.py +++ b/src/surface/flight_control/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/surface/keyboard_driver/test/test_keyboard_driver.py b/src/surface/flight_control/test/test_keyboard_control.py similarity index 78% rename from src/surface/keyboard_driver/test/test_keyboard_driver.py rename to src/surface/flight_control/test/test_keyboard_control.py index 0aa347ae..eb40872f 100644 --- a/src/surface/keyboard_driver/test/test_keyboard_driver.py +++ b/src/surface/flight_control/test/test_keyboard_control.py @@ -1,4 +1,4 @@ -from keyboard_driver.keyboard_driver_node import KeyboardListenerNode +from flight_control.keyboard_control_node import KeyboardListenerNode import rclpy import pytest diff --git a/src/surface/flight_control/test/test_manual_control.py b/src/surface/flight_control/test/test_manual_control.py new file mode 100644 index 00000000..d3358923 --- /dev/null +++ b/src/surface/flight_control/test/test_manual_control.py @@ -0,0 +1,42 @@ +import rclpy +from mavros_msgs.msg import OverrideRCIn +from flight_control.manual_control_node import ManualControlNode +from flight_control.pixhawk_instruction import ( + PixhawkInstruction, FORWARD_CHANNEL, THROTTLE_CHANNEL, + LATERAL_CHANNEL, PITCH_CHANNEL, YAW_CHANNEL, ROLL_CHANNEL, + ZERO_SPEED, RANGE_SPEED +) + + +def test_manual_control_instantiation() -> None: + """Unit test for the Manual Control instantiation.""" + rclpy.init() + ManualControlNode() + rclpy.shutdown() + + +def test_joystick_profiles() -> None: + """Unit test for the joystick_profiles function.""" + instruction = PixhawkInstruction( + # Nice boundary values + forward=0, + vertical=1, + lateral=-1, + + # Not nice possible values + pitch=0.34, + yaw=-0.6, + roll=0.92 + ) + + msg: OverrideRCIn = instruction.to_override_rc_in() + + assert msg.channels[FORWARD_CHANNEL] == ZERO_SPEED + assert msg.channels[THROTTLE_CHANNEL] == (ZERO_SPEED + RANGE_SPEED) + assert msg.channels[LATERAL_CHANNEL] == (ZERO_SPEED - RANGE_SPEED) + + # 1539 1378 + + assert msg.channels[PITCH_CHANNEL] == ZERO_SPEED + int(RANGE_SPEED * 0.34) + assert msg.channels[YAW_CHANNEL] == ZERO_SPEED + int(RANGE_SPEED * -0.6) + assert msg.channels[ROLL_CHANNEL] == ZERO_SPEED + int(RANGE_SPEED * 0.92) diff --git a/src/surface/keyboard_driver/test/test_mypy.py b/src/surface/flight_control/test/test_mypy.py similarity index 57% rename from src/surface/keyboard_driver/test/test_mypy.py rename to src/surface/flight_control/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/surface/keyboard_driver/test/test_mypy.py +++ b/src/surface/flight_control/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/keyboard_driver/test/test_pep257.py b/src/surface/flight_control/test/test_pep257.py similarity index 80% rename from src/surface/keyboard_driver/test/test_pep257.py rename to src/surface/flight_control/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/surface/keyboard_driver/test/test_pep257.py +++ b/src/surface/flight_control/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/gui/gui/app.py b/src/surface/gui/gui/app.py index 82a80f14..afcd2aed 100644 --- a/src/surface/gui/gui/app.py +++ b/src/surface/gui/gui/app.py @@ -21,11 +21,7 @@ def __init__(self, node_name: str) -> None: self.node.declare_parameter('theme', '') self.resize(1850, 720) - atexit.register(self.clean_shutdown) - - def clean_shutdown(self) -> None: - if rclpy.utilities.ok(): - rclpy.shutdown() + atexit.register(clean_shutdown) def run_gui(self) -> None: # Kills with Control + C @@ -45,3 +41,8 @@ def run_gui(self) -> None: # TODO: when the app closes it causes an error. Make not cause error? self.app.exec() + + +def clean_shutdown() -> None: + if rclpy.utilities.ok(): + rclpy.shutdown() diff --git a/src/surface/gui/gui/event_nodes/client.py b/src/surface/gui/gui/event_nodes/client.py index 9edbc76a..3579c5fb 100644 --- a/src/surface/gui/gui/event_nodes/client.py +++ b/src/surface/gui/gui/event_nodes/client.py @@ -15,7 +15,7 @@ class GUIEventClient(Node): # Set to None for no timeout limits on service requests # else set to float number of seconds to limit request spinning def __init__(self, srv_type: SrvType, topic: str, signal: pyqtBoundSignal, - timeout: float = 1.0): + timeout: float = 1.0, expected_namespace: str = '/surface/gui'): # Name this node with a sanitized version of the topic self.name: str = f'client_{re.sub(r"[^a-zA-Z0-9_]", "_", topic)}' @@ -25,6 +25,7 @@ def __init__(self, srv_type: SrvType, topic: str, signal: pyqtBoundSignal, self.topic: str = topic self.signal: pyqtBoundSignal = signal self.timeout = timeout + self.expected_namespace = expected_namespace self.cli: Client = self.create_client(srv_type, topic) Thread(target=self.__connect_to_service, daemon=True, @@ -35,10 +36,9 @@ def __init__(self, srv_type: SrvType, topic: str, signal: pyqtBoundSignal, def __connect_to_service(self) -> None: """Connect this client to a server in a separate thread.""" while not self.cli.wait_for_service(timeout_sec=self.timeout): - # TODO: include namespaces wherever we echo the topic self.get_logger().info( 'Service for GUI event client node on topic' - f' {self.topic} unavailable, waiting again...') + f' {self.expected_namespace}/{self.topic} unavailable, waiting again...') def send_request_async(self, request: SrvTypeRequest) -> None: """Send request to server in separate thread.""" diff --git a/src/surface/gui/gui/widgets/arm.py b/src/surface/gui/gui/widgets/arm.py index 0875f655..68288274 100644 --- a/src/surface/gui/gui/widgets/arm.py +++ b/src/surface/gui/gui/widgets/arm.py @@ -49,7 +49,8 @@ def __init__(self) -> None: self.arm_client: GUIEventClient = GUIEventClient( CommandBool, "mavros/cmd/arming", - self.signal + self.signal, + expected_namespace='/tether' ) def arm_clicked(self) -> None: diff --git a/src/surface/gui/gui/widgets/task_selector.py b/src/surface/gui/gui/widgets/task_selector.py index b7e0be6a..fdcd9b60 100644 --- a/src/surface/gui/gui/widgets/task_selector.py +++ b/src/surface/gui/gui/widgets/task_selector.py @@ -1,23 +1,18 @@ from gui.event_nodes.client import GUIEventClient -# from gui.event_nodes.subscriber import GUIEventSubscriber -from PyQt6.QtCore import pyqtSignal -# from PyQt6.QtCore import pyqtSlot +from PyQt6.QtCore import pyqtSignal, pyqtSlot from PyQt6.QtWidgets import QGridLayout, QLabel, QPushButton, QWidget -from task_selector.tasks import Tasks -# from rov_msgs.msg import TaskFeedback -from rov_msgs.srv import TaskRequest +from rov_msgs.srv import AutonomousFlight WIDTH = 200 class TaskSelector(QWidget): - """Qwidget that handles task selection with a dropdown.""" + """QWidget that handles task selection with a dropdown.""" # Declare signals with "object" params b/c we don't have access to # the ROS service object TaskRequest_Response - scheduler_response_signal: pyqtSignal = pyqtSignal(TaskRequest.Response) - # update_task_dropdown_signal: pyqtSignal = pyqtSignal(TaskFeedback) + scheduler_response_signal: pyqtSignal = pyqtSignal(AutonomousFlight.Response) def __init__(self) -> None: super().__init__() @@ -26,77 +21,57 @@ def __init__(self) -> None: self.setLayout(layout) # Create Start button - self.start_btn = QPushButton("Auto Docking") + self.start_btn = QPushButton("Start Auto Docking") self.start_btn.clicked.connect(self.start_btn_clicked) self.start_btn.setFixedHeight(75) self.start_btn.setFixedWidth(WIDTH) # Create Stop button - self.stop_btn = QPushButton("Manual Control") - self.stop_btn.clicked.connect(self.manual_control_btn_clicked) + self.stop_btn = QPushButton("Stop Auto Docking") + self.stop_btn.clicked.connect(self.stop_btn_clicked) self.stop_btn.setFixedHeight(75) self.stop_btn.setFixedWidth(WIDTH) # Add 'Task: ' label - self.task_label: QLabel = QLabel() - self.task_label.setFixedWidth(WIDTH) - self.task_label.setText('Task: Manual Control') + self.task_status: QLabel = QLabel() + self.task_status.setFixedWidth(WIDTH) + self.task_status.setText('Auto Docking: Disabled') # Setup Grid - layout.addWidget(self.task_label, 1, 1, 2, 2) + layout.addWidget(self.task_status, 1, 1, 2, 2) layout.addWidget(self.start_btn, 2, 1) layout.addWidget(self.stop_btn, 3, 1) - # Create ROS nodes # - # Create client (in separate thread to let GUI load before it connects) - # self.scheduler_response_signal.connect( - # self.handle_scheduler_response) - self.task_changed_client: GUIEventClient = GUIEventClient( - TaskRequest, 'task_request', self.scheduler_response_signal) - - # Server doesn't spin, so we init in main thread - # self.update_task_dropdown_signal.connect(self.update_task_dropdown) - # self.task_changed_server: GUIEventSubscriber = GUIEventSubscriber( - # TaskFeedback, '/task_feedback', self.update_task_dropdown_signal) + self.scheduler_response_signal.connect(self.handle_scheduler_response) + self.task_controller: GUIEventClient = GUIEventClient( + AutonomousFlight, 'auto_docker_control', self.scheduler_response_signal) def start_btn_clicked(self) -> None: """Tell the back about the user selecting the start button.""" - # Cancel change if task changer hasn't connected yet - # if not self.task_changed_client.connected: - # return - - self.task_changed_client.get_logger().info( + self.task_controller.get_logger().info( 'GUI changed task to: Auto Docking') - self.task_label.setText('Task: Auto Docking') + self.task_status.setText('Auto Docking: Enabled') - self.task_changed_client.send_request_async( - TaskRequest.Request(task_id=Tasks.AUTO_DOCKING.value)) + self.task_controller.send_request_async( + AutonomousFlight.Request(start=True)) - def manual_control_btn_clicked(self) -> None: + def stop_btn_clicked(self) -> None: """Tell the back about the user selecting the manual control button.""" - # Cancel change if task changer hasn't connected yet - # if not self.task_changed_client.connected: - # return - - self.task_changed_client.get_logger().info( + self.task_controller.get_logger().info( 'GUI changed task to: Manual Control') - self.task_label.setText('Task: Manual Control') - - self.task_changed_client.send_request_async( - TaskRequest.Request(task_id=Tasks.MANUAL_CONTROL.value)) + self.task_status.setText('Auto Docking: Disabled') - # @ pyqtSlot(TaskRequest.Response) - # def handle_scheduler_response(self, response: TaskRequest.Response): - # """Handle scheduler response to request sent from gui_changed_task.""" - # self.task_changed_server.get_logger().info(response.response) + self.task_controller.send_request_async( + AutonomousFlight.Request(start=False)) - # # TODO @Ben do we still use this? - # @ pyqtSlot(TaskFeedback) - # def update_task_dropdown(self, message: TaskFeedback): - # """Update the task selector dropdown when task changed by scheduler.""" - # self.combo_box.setCurrentIndex(message.task_id) - # self.task_changed_server.get_logger().info( - # f'GUI received task changed to: {self.combo_box.currentText()}' + - # f' at {self.combo_box.currentIndex()}') + @pyqtSlot(AutonomousFlight.Response) + def handle_scheduler_response(self, response: AutonomousFlight.Response) -> None: + """Handle scheduler response to request sent from gui_changed_task.""" + msg = 'Auto docking is ' + if response.is_running: + msg += 'now running' + else: + msg += 'no longer running' + self.task_controller.get_logger().info(msg) diff --git a/src/surface/gui/launch/operator_launch.py b/src/surface/gui/launch/operator_launch.py index 1f9f4dec..51767847 100644 --- a/src/surface/gui/launch/operator_launch.py +++ b/src/surface/gui/launch/operator_launch.py @@ -12,8 +12,8 @@ def generate_launch_description() -> LaunchDescription: parameters=[{'theme': LaunchConfiguration('theme', default='dark')}], remappings=[("/surface/gui/bottom_cam/image_raw", "/tether/bottom_cam/image_raw"), ("/surface/gui/task_request", "/surface/task_request"), - ("/surface/gui/task_feedback", "/surface/task_feedback") - ], + ("/surface/gui/task_feedback", "/surface/task_feedback"), + ("/surface/gui/auto_docker_control", "/surface/auto_docker_control")], emulate_tty=True, output='screen' ) diff --git a/src/surface/gui/setup.py b/src/surface/gui/setup.py index 27e84376..8796a58c 100644 --- a/src/surface/gui/setup.py +++ b/src/surface/gui/setup.py @@ -1,25 +1,26 @@ +"""setup.py for the gui module.""" import os from glob import glob from setuptools import setup -package_name = 'gui' +PACKAGE_NAME = 'gui' setup( - name=package_name, + name=PACKAGE_NAME, version='1.0.0', - packages=[package_name, os.path.join(package_name, 'widgets'), - os.path.join(package_name, 'event_nodes')], + packages=[PACKAGE_NAME, os.path.join(PACKAGE_NAME, 'widgets'), + os.path.join(PACKAGE_NAME, 'event_nodes')], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.[pxy][yma]*')) ], install_requires=['setuptools', 'pyqt6', 'pyqtdarktheme', 'opencv-python>=4.8.1', - 'numpy>=1.26', 'pytest-qt', 'pytest-xvfb', 'flake8==4.0.1', 'mypy >= 1.7'], + 'numpy>=1.26', 'pytest-qt', 'pytest-xvfb', 'flake8==4.0.1', 'mypy>=1.7'], zip_safe=True, maintainer='Benjamin Poulin', maintainer_email='bwp18@case.edu', diff --git a/src/surface/gui/test/test_flake8.py b/src/surface/gui/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/surface/gui/test/test_flake8.py +++ b/src/surface/gui/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/surface/gui/test/test_mypy.py b/src/surface/gui/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/surface/gui/test/test_mypy.py +++ b/src/surface/gui/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/gui/test/test_pep257.py b/src/surface/gui/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/surface/gui/test/test_pep257.py +++ b/src/surface/gui/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/keyboard_driver/README.md b/src/surface/keyboard_driver/README.md deleted file mode 100644 index 8ba50d07..00000000 --- a/src/surface/keyboard_driver/README.md +++ /dev/null @@ -1,35 +0,0 @@ -# keyboard_driver - -## Overview - -This package allows robot control from the keyboard. - -## Installation - -```bash -pip install pynput -``` - -## Usage - -Run the main node with - -```bash -ros2 launch keyboard_driver keyboard_driver_launch.py -``` - -## Launch files - -* **keyboard_driver_launch.py:** Launches the keyboard driver node which reads keyboard input to control robots. - -## Nodes - -### Keyboard Driver - -Reads keyboard input to control robots. - -#### Published Topics - -* **`/manual_control`** (ROVControl - an old, deleted message type) - - Outdated ROV movement instructions \ No newline at end of file diff --git a/src/surface/keyboard_driver/launch/keyboard_driver_launch.py b/src/surface/keyboard_driver/launch/keyboard_driver_launch.py deleted file mode 100644 index 59f6b290..00000000 --- a/src/surface/keyboard_driver/launch/keyboard_driver_launch.py +++ /dev/null @@ -1,16 +0,0 @@ -from launch.launch_description import LaunchDescription -import launch_ros.actions - - -def generate_launch_description() -> LaunchDescription: - return LaunchDescription( - [ - launch_ros.actions.Node( - package="keyboard_driver", - executable="keyboard_driver_node", - output="screen", - name="keyboard_driver_node", - emulate_tty=True - ), - ] - ) diff --git a/src/surface/keyboard_driver/package.xml b/src/surface/keyboard_driver/package.xml deleted file mode 100644 index 05e1dbe2..00000000 --- a/src/surface/keyboard_driver/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - keyboard_driver - 1.0.0 - MATE ROV keyboard driver - Seongmin Jung - Apache License 2.0 - - rclpy - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - rov_msgs - - pynput-pip - - - ament_python - - diff --git a/src/surface/keyboard_driver/setup.cfg b/src/surface/keyboard_driver/setup.cfg deleted file mode 100644 index 7296520a..00000000 --- a/src/surface/keyboard_driver/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/keyboard_driver -[install] -install_scripts=$base/lib/keyboard_driver diff --git a/src/surface/keyboard_driver/setup.py b/src/surface/keyboard_driver/setup.py deleted file mode 100644 index b0098ea3..00000000 --- a/src/surface/keyboard_driver/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -import os -from glob import glob - -from setuptools import setup - -package_name = "keyboard_driver" - -setup( - name=package_name, - version="1.0.0", - packages=[package_name], - data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - (os.path.join("share", package_name, "launch"), glob("launch/*.py")), - ], - install_requires=["setuptools", "pynput"], - zip_safe=True, - maintainer="Seongmin Jung", - maintainer_email="sxj754@case.edu", - description="MATE ROV keyboard driver", - license="Apache License 2.0", - tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "keyboard_driver_node = keyboard_driver.keyboard_driver_node:main", - ], - }, -) diff --git a/src/surface/ps5_controller/setup.py b/src/surface/ps5_controller/setup.py index 85fe33ac..d844a8c7 100644 --- a/src/surface/ps5_controller/setup.py +++ b/src/surface/ps5_controller/setup.py @@ -1,20 +1,21 @@ +"""setup.py for the ps5_controller module.""" import os from glob import glob from setuptools import setup -package_name = 'ps5_controller' +PACKAGE_NAME = 'ps5_controller' setup( - name=package_name, + name=PACKAGE_NAME, version='1.0.0', - packages=[package_name], + packages=[PACKAGE_NAME], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.[pxy][yma]*')) ], install_requires=['setuptools', 'flake8==4.0.1', 'mypy >= 1.7'], diff --git a/src/surface/ps5_controller/test/test_flake8.py b/src/surface/ps5_controller/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/surface/ps5_controller/test/test_flake8.py +++ b/src/surface/ps5_controller/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/surface/ps5_controller/test/test_mypy.py b/src/surface/ps5_controller/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/surface/ps5_controller/test/test_mypy.py +++ b/src/surface/ps5_controller/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/ps5_controller/test/test_pep257.py b/src/surface/ps5_controller/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/surface/ps5_controller/test/test_pep257.py +++ b/src/surface/ps5_controller/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/ros2_video_streamer b/src/surface/ros2_video_streamer index ec5f178b..0c7d9a1c 160000 --- a/src/surface/ros2_video_streamer +++ b/src/surface/ros2_video_streamer @@ -1 +1 @@ -Subproject commit ec5f178bcc56df34f83aa157e6f691c2ef45503c +Subproject commit 0c7d9a1cb9d15abb115282939e66d1f92d2fdad5 diff --git a/src/surface/rov_gazebo/launch/sim_launch.py b/src/surface/rov_gazebo/launch/sim_launch.py index 62c2a5a6..9e109abd 100644 --- a/src/surface/rov_gazebo/launch/sim_launch.py +++ b/src/surface/rov_gazebo/launch/sim_launch.py @@ -7,7 +7,7 @@ from launch.substitutions import Command from launch_ros.actions import Node -NS = "simulation" +NAMESPACE = "simulation" def generate_launch_description() -> LaunchDescription: @@ -32,7 +32,7 @@ def generate_launch_description() -> LaunchDescription: executable="robot_state_publisher", output="screen", parameters=[params], - namespace=NS, + namespace=NAMESPACE, emulate_tty=True ) @@ -41,8 +41,8 @@ def generate_launch_description() -> LaunchDescription: executable="robot_state_publisher", output="screen", parameters=[pool_params], - namespace=NS, - remappings=[(f"/{NS}/robot_description", f"/{NS}/pool_description")], + namespace=NAMESPACE, + remappings=[(f"/{NAMESPACE}/robot_description", f"/{NAMESPACE}/pool_description")], emulate_tty=True ) @@ -67,7 +67,7 @@ def generate_launch_description() -> LaunchDescription: "-allow_renaming", "true", ], - namespace=NS, + namespace=NAMESPACE, emulate_tty=True ) @@ -83,7 +83,7 @@ def generate_launch_description() -> LaunchDescription: "-allow_renaming", "true", ], - namespace=NS, + namespace=NAMESPACE, emulate_tty=True ) @@ -96,15 +96,15 @@ def generate_launch_description() -> LaunchDescription: executable="keyboard_driver_node", output="screen", name="keyboard_driver_node", - namespace=NS, - remappings=[(f"/{NS}/manual_control", "/manual_control")], + namespace=NAMESPACE, + remappings=[(f"/{NAMESPACE}/manual_control", "/manual_control")], emulate_tty=True ) # cam_bridge = Node( # package="ros_gz_bridge", # executable="parameter_bridge", - # namespace=NS, + # namespace=NAMESPACE, # name="cam_bridge", # arguments=[ # "/bottom_cam/image_raw@sensor_msgs/msg/Image@gz.msgs.Image", @@ -116,13 +116,13 @@ def generate_launch_description() -> LaunchDescription: # "/depth_cam/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked", # ], # remappings=[ - # (f"/{NS}/bottom_cam/image_raw", "/bottom_cam/image_raw"), - # (f"/{NS}/bottom_cam/camera_info", "/bottom_cam/camera_info"), - # (f"/{NS}/front_cam/image_raw", "/front_cam/image_raw"), - # (f"/{NS}/front_cam/camera_info", "/front_cam/camera_info"), - # (f"/{NS}/manip_cam/image_raw", "/manip_cam/image_raw"), - # (f"/{NS}/depth_cam", "/depth_cam/image_raw"), - # (f"/{NS}/depth_cam/points", "/depth_cam/points"), + # (f"/{NAMESPACE}/bottom_cam/image_raw", "/bottom_cam/image_raw"), + # (f"/{NAMESPACE}/bottom_cam/camera_info", "/bottom_cam/camera_info"), + # (f"/{NAMESPACE}/front_cam/image_raw", "/front_cam/image_raw"), + # (f"/{NAMESPACE}/front_cam/camera_info", "/front_cam/camera_info"), + # (f"/{NAMESPACE}/manip_cam/image_raw", "/manip_cam/image_raw"), + # (f"/{NAMESPACE}/depth_cam", "/depth_cam/image_raw"), + # (f"/{NAMESPACE}/depth_cam/points", "/depth_cam/points"), # ], # output="screen", # emulate_tty=True diff --git a/src/surface/rov_gazebo/setup.py b/src/surface/rov_gazebo/setup.py index 550e659f..33f49162 100644 --- a/src/surface/rov_gazebo/setup.py +++ b/src/surface/rov_gazebo/setup.py @@ -1,22 +1,23 @@ +"""setup.py for the rov_gazebo module.""" import os from glob import glob from setuptools import setup -package_name = "rov_gazebo" +PACKAGE_NAME = "rov_gazebo" setup( - name=package_name, + name=PACKAGE_NAME, version="1.0.0", - packages=[package_name], + packages=[PACKAGE_NAME], data_files=[ - ("share/ament_index/resource_index/packages", ["resource/" + package_name]), - ("share/" + package_name, ["package.xml"]), - (os.path.join("share", package_name, "launch"), glob("launch/*.py")), - (os.path.join("share", package_name, "description"), glob("description/*")), - (os.path.join("share", package_name, "config"), glob("config/*")), - (os.path.join("share", package_name, "worlds"), glob("worlds/*")), - (os.path.join("share", package_name, "meshes"), glob("meshes/*")), + ("share/ament_index/resource_index/packages", ["resource/" + PACKAGE_NAME]), + ("share/" + PACKAGE_NAME, ["package.xml"]), + (os.path.join("share", PACKAGE_NAME, "launch"), glob("launch/*.py")), + (os.path.join("share", PACKAGE_NAME, "description"), glob("description/*")), + (os.path.join("share", PACKAGE_NAME, "config"), glob("config/*")), + (os.path.join("share", PACKAGE_NAME, "worlds"), glob("worlds/*")), + (os.path.join("share", PACKAGE_NAME, "meshes"), glob("meshes/*")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/src/surface/rov_gazebo/test/test_flake8.py b/src/surface/rov_gazebo/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/surface/rov_gazebo/test/test_flake8.py +++ b/src/surface/rov_gazebo/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/surface/rov_gazebo/test/test_mypy.py b/src/surface/rov_gazebo/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/surface/rov_gazebo/test/test_mypy.py +++ b/src/surface/rov_gazebo/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/rov_gazebo/test/test_pep257.py b/src/surface/rov_gazebo/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/surface/rov_gazebo/test/test_pep257.py +++ b/src/surface/rov_gazebo/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/surface_main/launch/surface_operator_launch.py b/src/surface/surface_main/launch/surface_operator_launch.py index 691fa818..e194be8c 100644 --- a/src/surface/surface_main/launch/surface_operator_launch.py +++ b/src/surface/surface_main/launch/surface_operator_launch.py @@ -10,8 +10,8 @@ def generate_launch_description() -> LaunchDescription: gui_path: str = get_package_share_directory('gui') - task_selector_path: str = get_package_share_directory('task_selector') - # Launches Gui + flight_control_path: str = get_package_share_directory('flight_control') + # Launches GUI gui_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join( @@ -20,11 +20,11 @@ def generate_launch_description() -> LaunchDescription: ]), ) - # Launches Task Selector - task_selector_launch = IncludeLaunchDescription( + # Launches flight_control (auto docking, manual control, etc.) + flight_control_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ os.path.join( - task_selector_path, 'launch', 'task_scheduler_launch.py' + flight_control_path, 'launch', 'flight_control_launch.py' ) ]), ) @@ -33,7 +33,7 @@ def generate_launch_description() -> LaunchDescription: actions=[ PushRosNamespace("surface"), gui_launch, - task_selector_launch, + flight_control_launch, ] ) diff --git a/src/surface/surface_main/setup.py b/src/surface/surface_main/setup.py index 027b8146..83574567 100644 --- a/src/surface/surface_main/setup.py +++ b/src/surface/surface_main/setup.py @@ -1,20 +1,21 @@ +"""setup.py for the surface_main module.""" import os from glob import glob from setuptools import setup -package_name = 'surface_main' +PACKAGE_NAME = 'surface_main' setup( - name=package_name, + name=PACKAGE_NAME, version='1.0.0', - packages=[package_name], + packages=[PACKAGE_NAME], data_files=[ ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ['resource/' + PACKAGE_NAME]), + ('share/' + PACKAGE_NAME, ['package.xml']), # Include all launch files. - (os.path.join('share', package_name, 'launch'), + (os.path.join('share', PACKAGE_NAME, 'launch'), glob('launch/*launch.[pxy][yma]*')) ], install_requires=['setuptools', 'flake8==4.0.1', 'mypy >= 1.7'], diff --git a/src/surface/surface_main/test/test_flake8.py b/src/surface/surface_main/test/test_flake8.py index eac16eef..8ae474ef 100644 --- a/src/surface/surface_main/test/test_flake8.py +++ b/src/surface/surface_main/test/test_flake8.py @@ -1,3 +1,4 @@ +"""Test flake8 on this module.""" # Copyright 2017 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +20,8 @@ @pytest.mark.flake8 @pytest.mark.linter def test_flake8() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ + """Tests flake8 on this module.""" + error_code, errors = main_with_errors(argv=[]) + assert error_code == 0, \ + f'Found {len(errors)} code style errors / warnings:\n' + \ '\n'.join(errors) diff --git a/src/surface/surface_main/test/test_mypy.py b/src/surface/surface_main/test/test_mypy.py index fbd775a9..79fc51ad 100644 --- a/src/surface/surface_main/test/test_mypy.py +++ b/src/surface/surface_main/test/test_mypy.py @@ -1,3 +1,4 @@ +"""Test mypy on this module.""" import os import pytest @@ -7,7 +8,8 @@ @pytest.mark.mypy @pytest.mark.linter def test_mypy() -> None: + """Tests mypy on this module.""" file_path = __file__.replace(f'{__name__}.py', '') config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' + error_code = main(argv=['--config', config_file]) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/surface_main/test/test_pep257.py b/src/surface/surface_main/test/test_pep257.py index b6808e1d..b95ea531 100644 --- a/src/surface/surface_main/test/test_pep257.py +++ b/src/surface/surface_main/test/test_pep257.py @@ -1,3 +1,4 @@ +"""Test pep257 on this module.""" # Copyright 2015 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,5 +20,6 @@ @pytest.mark.linter @pytest.mark.pep257 def test_pep257() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + """Tests pep257 on this module.""" + error_code = main(argv=['.', 'test']) + assert error_code == 0, 'Found code style errors / warnings' diff --git a/src/surface/task_selector/README.md b/src/surface/task_selector/README.md deleted file mode 100644 index 67732e5c..00000000 --- a/src/surface/task_selector/README.md +++ /dev/null @@ -1,17 +0,0 @@ -# Task Selector - -TODO: REWRITE WHEN THIS WHOLE PACKAGE CHANGES - -A graph of the nodes used in the task scheduler - -![Scheduler Node Graph](doc/images/SelectorNodeGraph.png) - -The [task scheduler](task_selector/task_selector.py) -(node name task_selector) advertises a service named `task_request`. Any client can pass a task id, defined by the enumerator Tasks, to this service, and the scheduler will cancel any currently running task to switch to the newly requested task. An example series of requests is found in the [example request client](task_selector/example_request_client.py). - -List of [tasks](task_selector/tasks.py) - -* CANCEL: Cancel current task -* EX_BASIC: Example- do nothing and return successful -* EX_TIMED: Example- run a 10 second timer -* EX_GOOD_MORNING: Example- return a greeting depending on the time of day and level of cheeriness diff --git a/src/surface/task_selector/launch/task_scheduler_launch.py b/src/surface/task_selector/launch/task_scheduler_launch.py deleted file mode 100644 index eb6a9f3d..00000000 --- a/src/surface/task_selector/launch/task_scheduler_launch.py +++ /dev/null @@ -1,56 +0,0 @@ -from launch.launch_description import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - - # launches main task scheduler - selector_node: Node = Node( - package='task_selector', - executable='selector', - emulate_tty=True, - output='screen' - ) - - # Manual Control - manual_control_node: Node = Node( - package='task_selector', - executable='manual_control_node', - remappings=[('/surface/manipulator_control', '/tether/manipulator_control'), - ('/surface/mavros/rc/override', '/tether/mavros/rc/override')], - emulate_tty=True, - output='screen' - ) - - # # example of node requesting tasks - # ex_request_client_node: Node = Node( - # package='task_selector', - # executable='ex_request_client' - # ) - - # # example task- run a 10 second timer - # ex_timed_task_node: Node = Node( - # package='task_selector', - # executable='ex_timed_task' - # ) - - # # example task- say the task is finished - # ex_basic_task_node: Node = Node( - # package='task_selector', - # executable='ex_basic_task' - # ) - - # # example task- say good morning - # ex_morning_task_node: Node = Node( - # package='task_selector', - # executable='ex_morning_task' - # ) - - return LaunchDescription([ - selector_node, - manual_control_node, - # ex_request_client_node, - # ex_timed_task_node, - # ex_basic_task_node, - # ex_morning_task_node - ]) diff --git a/src/surface/task_selector/resource/task_selector b/src/surface/task_selector/resource/task_selector deleted file mode 100644 index e69de29b..00000000 diff --git a/src/surface/task_selector/setup.cfg b/src/surface/task_selector/setup.cfg deleted file mode 100644 index eaf889a2..00000000 --- a/src/surface/task_selector/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/task_selector -[install] -install_scripts=$base/lib/task_selector diff --git a/src/surface/task_selector/setup.py b/src/surface/task_selector/setup.py deleted file mode 100644 index ab8dbbee..00000000 --- a/src/surface/task_selector/setup.py +++ /dev/null @@ -1,37 +0,0 @@ -import os -from glob import glob - -from setuptools import setup - -package_name = 'task_selector' - -setup( - name=package_name, - version='1.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - # Include all launch files. - (os.path.join('share', package_name, 'launch'), - glob('launch/*launch.[pxy][yma]*')) - ], - install_requires=['setuptools', 'flake8==4.0.1', 'mypy >= 1.7'], - zip_safe=True, - maintainer='ericy', - maintainer_email='ery12@case.edu', - description='Mate ROV task scheduler and task nodes', - license='Apache License 2.0', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'selector = task_selector.task_selector:main', - 'manual_control_node = task_selector.manual_control_node:main', - 'ex_request_client = task_selector.example_request_client:main', - 'ex_timed_task = task_selector.basic_task_timed_node:main', - 'ex_basic_task = task_selector.basic_task_node:main', - 'ex_morning_task = task_selector.is_morning_node:main', - ], - }, -) diff --git a/src/surface/task_selector/task_selector/__init__.py b/src/surface/task_selector/task_selector/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/src/surface/task_selector/task_selector/basic_task_node.py b/src/surface/task_selector/task_selector/basic_task_node.py deleted file mode 100644 index 73b04386..00000000 --- a/src/surface/task_selector/task_selector/basic_task_node.py +++ /dev/null @@ -1,50 +0,0 @@ -import rclpy -from rclpy.action.server import ActionServer, CancelResponse, ServerGoalHandle -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node - -from rov_msgs.action import BasicTask - - -class BasicTaskNode(Node): - - def __init__(self) -> None: - super().__init__('basic_task_node', - parameter_overrides=[]) - self._action_server = ActionServer( - self, - BasicTask, - 'example_task', - self.execute_callback - ) - - def execute_callback(self, goal_handle: ServerGoalHandle) -> BasicTask.Result: - self.get_logger().info('Executing goal...') - - if goal_handle.is_cancel_requested: - goal_handle.canceled() - self.get_logger().info('Goal canceled') - return BasicTask.Result() - else: - feedback_msg = BasicTask.Feedback() - feedback_msg.feedback_message = "Task is executing" - - self.get_logger().info('Feedback:' + feedback_msg.feedback_message) - goal_handle.publish_feedback(feedback_msg) - - goal_handle.succeed() - - result = BasicTask.Result() - return result - - def cancel_callback(self, goal_handle: ServerGoalHandle) -> CancelResponse: - self.get_logger().info('Received cancel request') - return CancelResponse.ACCEPT - - -def main() -> None: - rclpy.init() - - task_controller = BasicTaskNode() - executor = MultiThreadedExecutor() - rclpy.spin(task_controller, executor=executor) diff --git a/src/surface/task_selector/task_selector/basic_task_timed_node.py b/src/surface/task_selector/task_selector/basic_task_timed_node.py deleted file mode 100644 index c2156c4b..00000000 --- a/src/surface/task_selector/task_selector/basic_task_timed_node.py +++ /dev/null @@ -1,56 +0,0 @@ -import time - -import rclpy -from rclpy.action.server import ActionServer, CancelResponse, ServerGoalHandle -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node - -from rov_msgs.action import BasicTask - - -class BasicTaskTimedNode(Node): - - def __init__(self) -> None: - super().__init__('basic_task_timed', - parameter_overrides=[]) - self._action_server = ActionServer( - self, - BasicTask, - 'timed_task', - self.execute_callback, - cancel_callback=self.cancel_callback - ) - - def execute_callback(self, goal_handle: ServerGoalHandle) -> BasicTask.Result: - self.get_logger().info('Executing goal...') - - feedback_msg = BasicTask.Feedback() - for x in range(10): - if goal_handle.is_cancel_requested: - goal_handle.canceled() - self.get_logger().info('Goal canceled') - return BasicTask.Result() - else: - feedback_msg.feedback_message = str(10 - x) + " seconds left" - - self.get_logger().info( - 'Feedback:' + feedback_msg.feedback_message) - goal_handle.publish_feedback(feedback_msg) - time.sleep(1) - - goal_handle.succeed() - - result = BasicTask.Result() - return result - - def cancel_callback(self, goal_handle: ServerGoalHandle) -> CancelResponse: - self.get_logger().info('Received cancel request') - return CancelResponse.ACCEPT - - -def main() -> None: - rclpy.init() - - task_controller = BasicTaskTimedNode() - executor = MultiThreadedExecutor() - rclpy.spin(task_controller, executor=executor) diff --git a/src/surface/task_selector/task_selector/example_request_client.py b/src/surface/task_selector/task_selector/example_request_client.py deleted file mode 100644 index 8495f38d..00000000 --- a/src/surface/task_selector/task_selector/example_request_client.py +++ /dev/null @@ -1,49 +0,0 @@ -# import time - -import rclpy -from rclpy.node import Node -# from task_selector.tasks import Tasks - -from rov_msgs.srv import TaskRequest - - -class ExampleRequestClient(Node): - - def __init__(self) -> None: - super().__init__('example_request_client') - self.cli = self.create_client(TaskRequest, 'gui/task_request') - while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - self.req = TaskRequest.Request() - - # def send_request(self, task_id): - # enum_id = task_id.value - # self.req.task_id = enum_id - # self.future = self.cli.call_async(self.req) - # rclpy.spin_until_future_complete(self, self.future) - # return self.future.result() - - -def main() -> None: - rclpy.init() - - example_client = ExampleRequestClient() - - # example_client.get_logger().info("Sending timer request") - # response = example_client.send_request(Tasks.EX_TIMED) - # example_client.get_logger().info(response.response) - - # time.sleep(5) - - # example_client.get_logger().info("Sending morning request") - # response = example_client.send_request(Tasks.EX_GOOD_MORNING) - # example_client.get_logger().info(response.response) - - # time.sleep(2) - - # example_client.get_logger().info("Sending basic request") - # response = example_client.send_request(Tasks.EX_BASIC) - # example_client.get_logger().info(response.response) - - example_client.destroy_node() - rclpy.shutdown() diff --git a/src/surface/task_selector/task_selector/is_morning_node.py b/src/surface/task_selector/task_selector/is_morning_node.py deleted file mode 100644 index b51ba49e..00000000 --- a/src/surface/task_selector/task_selector/is_morning_node.py +++ /dev/null @@ -1,60 +0,0 @@ -import rclpy -from rclpy.action.server import ActionServer, ServerGoalHandle -from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node - -from rov_msgs.action import Example - - -class IsMorning(Node): - - def __init__(self) -> None: - super().__init__('good_morning_sayer', - parameter_overrides=[]) - self._action_server = ActionServer( - self, - Example, - 'say_good_morning', - self.execute_callback - ) - - def execute_callback(self, goal_handle: ServerGoalHandle) -> Example.Result: - self.get_logger().info('Executing goal...') - - if goal_handle.is_cancel_requested: - goal_handle.canceled() - self.get_logger().info('Goal canceled') - return Example.Result() - else: - feedback_msg = Example.Feedback() - feedback_msg.feedback_message = """I am thinking about what to say to you""" - - self.get_logger().info('Feedback:' + feedback_msg.feedback_message) - goal_handle.publish_feedback(feedback_msg) - - is_morning = goal_handle.request.morning - is_cheery = goal_handle.request.cheery - - if is_cheery: - message = 'Good' - else: - message = 'Not good' - - if is_morning: - message += ' morning!' - else: - message += ' not morning!' - - goal_handle.succeed() - - result = Example.Result() - result.message = message - return result - - -def main() -> None: - rclpy.init() - - task_controller = IsMorning() - executor = MultiThreadedExecutor() - rclpy.spin(task_controller, executor=executor) diff --git a/src/surface/task_selector/task_selector/task_selector.py b/src/surface/task_selector/task_selector/task_selector.py deleted file mode 100644 index 3839228e..00000000 --- a/src/surface/task_selector/task_selector/task_selector.py +++ /dev/null @@ -1,178 +0,0 @@ -import rclpy -from rclpy.action.client import ActionClient -from rclpy.node import Node -from rclpy.qos import qos_profile_system_default -from task_selector.tasks import Tasks - -# from rov_msgs.action import Example -from rov_msgs.action import BasicTask -from rov_msgs.msg import TaskFeedback -from rov_msgs.srv import TaskRequest - - -class TaskSelector(Node): - - def __init__(self) -> None: - # creation of a Node with its name as input - super().__init__('task_selector', - parameter_overrides=[]) - - # create service to handle requests for task switching - self.request_server = self.create_service( - TaskRequest, 'task_request', self.request_task_callback) - - self.feedback_server = self.create_publisher( - TaskFeedback, 'task_feedback', qos_profile_system_default) - - # instantiates new action clients with inputs of node, - # action type, action name - # self.morning_action_client = ActionClient(self, - # Example, - # 'say_good_morning') - # self.timed_task_client = ActionClient(self, - # BasicTask, - # 'timed_task') - # self.basic_task_client = ActionClient(self, - # BasicTask, - # 'example_task') - - self.manual_control_client = ActionClient(self, - BasicTask, - 'manual_control') - self.active = False - self._goal_handle = None - - # self.send_basic_goal(self.manual_control_client) - - def request_task_callback(self, request: TaskRequest.Request, - response: TaskRequest.Response) -> TaskRequest.Response: - response.response = "Acknowledged" - if self.active: - pass - # self.cancel_goal() - - if request.task_id == Tasks.MANUAL_CONTROL.value: - self.active = True - # self.send_basic_goal(self.manual_control_client) - # elif request.task_id == Tasks.EX_GOOD_MORNING.value: - # self.send_morning_goal(True, True) - # elif request.task_id == Tasks.EX_TIMED.value: - # self.send_basic_goal(self.timed_task_client) - # elif request.task_id == Tasks.EX_BASIC.value: - # self.send_basic_goal(self.basic_task_client) - elif request.task_id == Tasks.AUTO_DOCKING.value: - pass - elif request.task_id == Tasks.CANCEL.value: - response.response = "Canceled" - else: - response.response = "Invalid task id" - return response - - # Basic task client takes no input variables and only receives feedback, - # no result data - # - # send_basic_goal() takes a basic client and requests for the server it's - # attached to to run a task - # def send_basic_goal(self, client: ActionClient) -> None: - # goal_msg = BasicTask.Goal() - - # if not self.active: - # self.get_logger().info('Waiting for action server...') - # client.wait_for_server() - - # if not self.active: - # self.get_logger().info('Sending goal request...') - # self._send_goal_future = client.send_goal_async( - # goal_msg, feedback_callback=self.feedback_callback) - - # self._send_goal_future.add_done_callback(self.basic_response_callback) - - # # A Say Good Morning server takes the time of day and cheeriness to - # # produce a greeting - # def send_morning_goal(self, morning: bool, cheery: bool): - # goal_msg = Example.Goal() - # goal_msg.morning = morning - # goal_msg.cheery = cheery - - # self.get_logger().info('Waiting for action server...') - # self.morning_action_client.wait_for_server() - - # self.get_logger().info('Sending goal request...') - # self._send_goal_future = self.morning_action_client.send_goal_async( - # goal_msg, feedback_callback=self.feedback_callback) - # self._send_goal_future.add_done_callback( - # self.morning_response_callback) - - # Checks if goal was accepted - # TODO what type is future? - # def basic_response_callback(self, future): - # goal_handle = future.result() - # if not goal_handle.accepted: - # self.get_logger().info('Goal rejected') - # return - - # self.get_logger().info('Goal accepted') - - # self._goal_handle = goal_handle - - # self._get_result_future = goal_handle.get_result_async() - # self._get_result_future.add_done_callback(self.basic_result_callback) - - # def morning_response_callback(self, future): - # goal_handle = future.result() - # if not goal_handle.accepted: - # self.get_logger().info('Goal rejected') - # return - - # self.get_logger().info('Goal accepted') - - # self._get_result_future = goal_handle.get_result_async() - # self._get_result_future.add_done_callback(self.morning_result_callback) - - # Notify us that task is finished - # TODO what type is future? - # def basic_result_callback(self, future) -> None: - # self.get_logger().info("Task finished") - # self.active = False - - # # Logs greeting that the morning server sends - # def morning_result_callback(self, future): - # result = future.result().result - # self.get_logger().info('Result: {0}'.format(result.message)) - # self.active = False - - # Logs feedback from action server - # TODO what type is feedback_msg? - # def feedback_callback(self, feedback_msg) -> None: - # feedback = feedback_msg.feedback - # self.get_logger().info( - # 'Received feedback: {0}'.format(feedback.feedback_message)) - - # Only works if server runs on a multithreaded executor - # def cancel_goal(self) -> None: - # if self._goal_handle is None: - # self.get_logger().warn('Could not cancel goal because there is none') - # return - - # self.get_logger().info('Canceling goal') - # # Cancel the goal - # future = self._goal_handle.cancel_goal_async() - # future.add_done_callback(self.cancel_done) - - # Logs if goal was canceled - # TODO what type is future? - # def cancel_done(self, future) -> None: - # cancel_response = future.result() - # if len(cancel_response.goals_canceling) > 0: - # self.get_logger().info('Goal successfully canceled') - # self.active = False - # else: - # self.get_logger().info('Goal failed to cancel') - - -def main() -> None: - rclpy.init() - - action_client = TaskSelector() - - rclpy.spin(action_client) diff --git a/src/surface/task_selector/task_selector/tasks.py b/src/surface/task_selector/task_selector/tasks.py deleted file mode 100644 index ecf71270..00000000 --- a/src/surface/task_selector/task_selector/tasks.py +++ /dev/null @@ -1,11 +0,0 @@ -from enum import IntEnum - - -class Tasks(IntEnum): - MANUAL_CONTROL = 0 - AUTO_DOCKING = 1 - CANCEL = -1 - ERROR = -2 - # EX_BASIC = auto() - # EX_TIMED = auto() - # EX_GOOD_MORNING = auto() diff --git a/src/surface/task_selector/test/test_flake8.py b/src/surface/task_selector/test/test_flake8.py deleted file mode 100644 index eac16eef..00000000 --- a/src/surface/task_selector/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() -> None: - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/surface/task_selector/test/test_manual_control.py b/src/surface/task_selector/test/test_manual_control.py deleted file mode 100644 index cb838b24..00000000 --- a/src/surface/task_selector/test/test_manual_control.py +++ /dev/null @@ -1,25 +0,0 @@ -import rclpy -from task_selector.manual_control_node import ManualControlNode, ZERO_SPEED, RANGE_SPEED - - -def test_manual_control_instantiation() -> None: - """Unit test for the Manual Control instantiation.""" - rclpy.init() - ManualControlNode() - rclpy.shutdown() - - -def test_joystick_profiles() -> None: - """Unit test for the joystick_profiles function.""" - rclpy.init() - node = ManualControlNode() - - # Nice boundary values - assert node.joystick_profiles(0) == ZERO_SPEED - assert node.joystick_profiles(1) == (ZERO_SPEED + RANGE_SPEED) - assert node.joystick_profiles(-1) == (ZERO_SPEED - RANGE_SPEED) - - # Not nice possible values - assert node.joystick_profiles(0.34) == 1539 - assert node.joystick_profiles(-0.6) == 1378 - rclpy.shutdown() diff --git a/src/surface/task_selector/test/test_mypy.py b/src/surface/task_selector/test/test_mypy.py deleted file mode 100644 index fbd775a9..00000000 --- a/src/surface/task_selector/test/test_mypy.py +++ /dev/null @@ -1,13 +0,0 @@ -import os - -import pytest -from ament_mypy.main import main - - -@pytest.mark.mypy -@pytest.mark.linter -def test_mypy() -> None: - file_path = __file__.replace(f'{__name__}.py', '') - config_file = os.path.join(file_path, '..', '..', '..', '..', 'mypy.ini') - rc = main(argv=['--config', config_file]) - assert rc == 0, 'Found code style errors / warnings' diff --git a/src/surface/task_selector/test/test_pep257.py b/src/surface/task_selector/test/test_pep257.py deleted file mode 100644 index b6808e1d..00000000 --- a/src/surface/task_selector/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() -> None: - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/src/surface/task_selector/test/test_task_selector.py b/src/surface/task_selector/test/test_task_selector.py deleted file mode 100644 index 41758804..00000000 --- a/src/surface/task_selector/test/test_task_selector.py +++ /dev/null @@ -1,25 +0,0 @@ -import os - -import pytest -from ament_index_python.packages import get_package_share_directory -from launch.launch_description import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_testing.actions import ReadyToTest - - -@pytest.mark.launch_test -def generate_test_description() -> LaunchDescription: - - task_selector_path: str = get_package_share_directory('task_selector') - return LaunchDescription([ - # Launches Task Selector - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - os.path.join( - task_selector_path, 'launch', 'task_scheduler_launch.py' - ) - ]), - ), - ReadyToTest() - ])