From 2122d973ed64fb33c6d1c5d2fc5355184bf7758c Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 15 May 2023 21:20:55 -0700 Subject: [PATCH 1/6] Started integration of launch files and updated dockerfile to properly install mavros --- .devcontainer.json | 5 +- .docker/Dockerfile | 53 +++++++------ .dockerignore | 1 + blue_bringup/LICENSE | 17 ++++ blue_bringup/blue_bringup/__init__.py | 0 blue_bringup/config/blue.yaml | 47 +++++++++++ blue_bringup/launch/blue.launch.py | 107 ++++++++++++++++++++++++++ blue_bringup/package.xml | 21 +++++ blue_bringup/resource/blue_bringup | 0 blue_bringup/setup.cfg | 4 + blue_bringup/setup.py | 48 ++++++++++++ blue_bringup/test/test_copyright.py | 25 ++++++ blue_bringup/test/test_flake8.py | 25 ++++++ blue_bringup/test/test_pep257.py | 23 ++++++ blue_control/CMakeLists.txt | 4 + blue_control/launch/control.launch.py | 62 +++++++++++++++ blue_manager/blue_manager/manager.py | 2 + blue_manager/launch/manager.launch.py | 51 ++++++++++++ blue_manager/setup.py | 24 ++++++ 19 files changed, 493 insertions(+), 26 deletions(-) create mode 100644 blue_bringup/LICENSE create mode 100644 blue_bringup/blue_bringup/__init__.py create mode 100644 blue_bringup/config/blue.yaml create mode 100644 blue_bringup/launch/blue.launch.py create mode 100644 blue_bringup/package.xml create mode 100644 blue_bringup/resource/blue_bringup create mode 100644 blue_bringup/setup.cfg create mode 100644 blue_bringup/setup.py create mode 100644 blue_bringup/test/test_copyright.py create mode 100644 blue_bringup/test/test_flake8.py create mode 100644 blue_bringup/test/test_pep257.py create mode 100644 blue_control/launch/control.launch.py create mode 100644 blue_manager/launch/manager.launch.py diff --git a/.devcontainer.json b/.devcontainer.json index 461b19f5..d0ae4926 100644 --- a/.devcontainer.json +++ b/.devcontainer.json @@ -7,7 +7,7 @@ "ROS_DISTRO": "humble" } }, - "remoteUser": "dev", + "remoteUser": "blue", "runArgs": [ "--network=host", "--cap-add=SYS_PTRACE", @@ -91,9 +91,6 @@ "[xml]": { "editor.defaultFormatter": "redhat.vscode-xml" }, - "[yaml]": { - "editor.defaultFormatter": "redhat.vscode-yaml" - }, "[markdown]": { "editor.rulers": [80], "editor.defaultFormatter": "DavidAnson.vscode-markdownlint" diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 2dd764dd..5bafb13c 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -26,12 +26,6 @@ RUN apt-get -q update \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* -# Install MAVROS dependencies prior to installing ROS dependencies -RUN [ "/bin/bash" , "-c" , "\ - wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh \ - && chmod +x install_geographiclib_datasets.sh \ - && sudo ./install_geographiclib_datasets.sh" ] - # Install all ROS dependencies RUN apt-get -q update \ && apt-get -q -y upgrade \ @@ -42,6 +36,35 @@ RUN apt-get -q update \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* +# Configure a new non-root user +ARG USERNAME=blue +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ + && apt-get update && apt-get upgrade -y \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ + && chmod 0440 /etc/sudoers.d/$USERNAME \ + && rm -rf /var/lib/apt/lists/* \ + && echo "source /usr/share/bash-completion/completions/git" >> /home/$USERNAME/.bashrc + +# Switch to the non-root user to install MAVROS dependencies and ArduSub +USER $USERNAME +ENV USER=$USERNAME + +# Set the working directory to the user's home directory +WORKDIR /home/$USERNAME + +# Install MAVROS dependencies +RUN wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/install_geographiclib_datasets.sh \ + && chmod +x install_geographiclib_datasets.sh \ + && sudo ./install_geographiclib_datasets.sh + +# Switch back to root user +USER root +ENV USER=root + FROM ci as source ENV ROS_UNDERLAY /root/ws_blue/install @@ -105,20 +128,6 @@ RUN pip3 install \ black \ setuptools==58.2.0 -# Configure a new non-root user -ARG USERNAME=dev -ARG USER_UID=1000 -ARG USER_GID=$USER_UID - -RUN groupadd --gid $USER_GID $USERNAME \ - && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ - && apt-get update && apt-get upgrade -y \ - && apt-get install -y sudo \ - && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ - && chmod 0440 /etc/sudoers.d/$USERNAME \ - && rm -rf /var/lib/apt/lists/* \ - && echo "source /usr/share/bash-completion/completions/git" >> /home/$USERNAME/.bashrc \ - && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc - ARG WORKSPACE -RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/$USERNAME/.bashrc +RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/$USERNAME/.bashrc \ + && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc diff --git a/.dockerignore b/.dockerignore index fdcd7f9d..3860a782 100644 --- a/.dockerignore +++ b/.dockerignore @@ -6,3 +6,4 @@ !blue_manager !blue_control !blue_msgs +!blue_bringup diff --git a/blue_bringup/LICENSE b/blue_bringup/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_bringup/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/blue_bringup/blue_bringup/__init__.py b/blue_bringup/blue_bringup/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/blue_bringup/config/blue.yaml b/blue_bringup/config/blue.yaml new file mode 100644 index 00000000..1425054e --- /dev/null +++ b/blue_bringup/config/blue.yaml @@ -0,0 +1,47 @@ +blue_manager: + ros__parameters: + num_thrusters: 8 + mode_change_timeout: 1.0 + mode_change_retries: 3 + +ismc: + ros__parameters: + mass: 11.5 + buoyancy: 112.80 + weight: 114.80 + inertia_tensor_coeff: [0.16, 0.16, 0.16] + added_mass_coeff: [-5.50, -12.70, -14.60, -0.12, -0.12, -0.12] + linear_damping_coeff: [-4.03, -6.22, -5.18, -0.07, -0.07, -0.07] + quadratic_damping_coeff: [-18.18, -21.66, -36.99, -1.55, -1.55, -1.55] + center_of_gravity: [0.0, 0.0, 0.0] + center_of_buoyancy: [0.0, 0.0, 0.0] + ocean_current: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + num_thrusters: 8 + tcm: [0.707, 0.707, -0.707, -0.707, 0.0, 0.0, 0.0, 0.0, + -0.707, 0.707, -0.707, 0.707, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, -1.0, 1.0, 1.0, -1.0, + 0.06, -0.06, 0.06, -0.06, -0.218, -0.218, 0.218, 0.218, + 0.06, 0.06, -0.06, -0.06, 0.120, -0.120, 0.120, -0.120, + -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0] + +mavros: + ros__parameters: + system_id: 255 + plugin_allowlist: + - sys_status + - command + - imu + - local_position + - rc_io + - param + +mavros_node: + ros__parameters: + fcu_url: "tcp://localhost" + gcs_url: "udp://@localhost:14550" + +mavros/local_position: + ros__parameters: + frame_id: "map" + tf: + send: false diff --git a/blue_bringup/launch/blue.launch.py b/blue_bringup/launch/blue.launch.py new file mode 100644 index 00000000..3a14369b --- /dev/null +++ b/blue_bringup/launch/blue.launch.py @@ -0,0 +1,107 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description to run the system. + + Returns: + The Blue ROS 2 launch description. + """ + # Declare the launch arguments + args = [ + DeclareLaunchArgument( + "config", + default_value="blue.yaml", + description="The ROS 2 parameters configuration file", + ), + DeclareLaunchArgument( + "controller", + default_value="ismc", + description=( + "The controller to use; this should be the same name as the" + " controller's executable" + ), + choices=["ismc"], + ), + ] + + nodes = [ + Node( + package="mavros", + executable="mavros_node", + output="screen", + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("blue_bringup"), + "config", + LaunchConfiguration("config"), + ] + ), + ], + ), + ] + + # Declare additional launch files to run + includes = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("blue_manager"), "manager.launch.py"] + ) + ), + launch_arguments={ + "config_filepath": PathJoinSubstitution( + [ + FindPackageShare("blue_bringup"), + "config", + LaunchConfiguration("config"), + ] + ), + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("blue_control"), "launch", "control.launch.py"] + ) + ), + launch_arguments={ + "config_filepath": PathJoinSubstitution( + [ + FindPackageShare("blue_bringup"), + "config", + LaunchConfiguration("config"), + ] + ), + "controller": LaunchConfiguration("controller"), + }.items(), + ), + ] + + return LaunchDescription(args + nodes + includes) diff --git a/blue_bringup/package.xml b/blue_bringup/package.xml new file mode 100644 index 00000000..0ad818f5 --- /dev/null +++ b/blue_bringup/package.xml @@ -0,0 +1,21 @@ + + + + blue_bringup + 0.0.1 + Entrypoints for the Blue project. + Evan Palmer + MIT + + mavros + mavros_extras + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/blue_bringup/resource/blue_bringup b/blue_bringup/resource/blue_bringup new file mode 100644 index 00000000..e69de29b diff --git a/blue_bringup/setup.cfg b/blue_bringup/setup.cfg new file mode 100644 index 00000000..a03d75ab --- /dev/null +++ b/blue_bringup/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/blue_bringup +[install] +install_scripts=$base/lib/blue_bringup diff --git a/blue_bringup/setup.py b/blue_bringup/setup.py new file mode 100644 index 00000000..28cc8a4c --- /dev/null +++ b/blue_bringup/setup.py @@ -0,0 +1,48 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import os +from glob import glob + +from setuptools import setup + +package_name = "blue_bringup" + +setup( + name=package_name, + version="0.0.1", + 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), glob("launch/*.launch.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Evan Palmer", + maintainer_email="evanp922@gmail.com", + description="Entrypoints for the Blue project.", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/blue_bringup/test/test_copyright.py b/blue_bringup/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/blue_bringup/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/blue_bringup/test/test_flake8.py b/blue_bringup/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/blue_bringup/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/blue_bringup/test/test_pep257.py b/blue_bringup/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/blue_bringup/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index 679c2315..e2b77e42 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -69,6 +69,10 @@ install(DIRECTORY DESTINATION include ) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/blue_control/launch/control.launch.py b/blue_control/launch/control.launch.py new file mode 100644 index 00000000..88518528 --- /dev/null +++ b/blue_control/launch/control.launch.py @@ -0,0 +1,62 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the Blue control interface. + + Returns: + LaunchDescription: The Blue control launch description. + """ + args = [ + DeclareLaunchArgument( + "config_filepath", + default_value=None, + description="The path to the configuration YAML file", + ), + DeclareLaunchArgument( + "controller", + default_value="ismc", + description=( + "The controller to use; this should be the same name as the" + " controller's executable" + ), + choices=["ismc"], + ), + ] + + controller = LaunchConfiguration("controller") + + nodes = [ + Node( + package="blue_control", + executable=controller, + name=controller, + output="screen", + parameters=[LaunchConfiguration("config_filepath")], + ), + ] + + return LaunchDescription(args + nodes) diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index a7bc2b17..efb40f12 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -29,6 +29,8 @@ from rclpy.node import Node from std_srvs.srv import SetBool +# TODO(evan): Add message requests for pose, attitude, and battery + class Manager(Node): """Provides an interface between custom controllers and the BlueROV2.""" diff --git a/blue_manager/launch/manager.launch.py b/blue_manager/launch/manager.launch.py new file mode 100644 index 00000000..91956309 --- /dev/null +++ b/blue_manager/launch/manager.launch.py @@ -0,0 +1,51 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the Blue manager interface. + + Returns: + LaunchDescription: The Blue manager launch description. + """ + args = [ + DeclareLaunchArgument( + "config_filepath", + default_value=None, + description="The path to the configuration YAML file", + ), + ] + + nodes = [ + Node( + package="blue_manager", + executable="blue_manager", + name="blue_manager", + output="screen", + parameters=[LaunchConfiguration("config_filepath")], + ), + ] + + return LaunchDescription(args + nodes) diff --git a/blue_manager/setup.py b/blue_manager/setup.py index 372d6539..d35f2348 100644 --- a/blue_manager/setup.py +++ b/blue_manager/setup.py @@ -1,3 +1,26 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import os +from glob import glob + from setuptools import setup package_name = "blue_manager" @@ -9,6 +32,7 @@ data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name), glob("launch/*.launch.py")), ], install_requires=["setuptools"], zip_safe=True, From f22776a4be5e99863a5f7af4dab2e4cf5d36b6bf Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Mon, 15 May 2023 21:23:56 -0700 Subject: [PATCH 2/6] precommit --- blue_bringup/launch/blue.launch.py | 2 +- blue_bringup/test/test_copyright.py | 10 ++++++---- blue_bringup/test/test_flake8.py | 8 ++++---- blue_bringup/test/test_pep257.py | 6 +++--- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/blue_bringup/launch/blue.launch.py b/blue_bringup/launch/blue.launch.py index 3a14369b..6d3851d3 100644 --- a/blue_bringup/launch/blue.launch.py +++ b/blue_bringup/launch/blue.launch.py @@ -22,8 +22,8 @@ from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: diff --git a/blue_bringup/test/test_copyright.py b/blue_bringup/test/test_copyright.py index 97a39196..8f18fa4b 100644 --- a/blue_bringup/test/test_copyright.py +++ b/blue_bringup/test/test_copyright.py @@ -12,14 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_copyright.main import main import pytest +from ament_copyright.main import main # Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) @pytest.mark.copyright @pytest.mark.linter def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/blue_bringup/test/test_flake8.py b/blue_bringup/test/test_flake8.py index 27ee1078..f494570f 100644 --- a/blue_bringup/test/test_flake8.py +++ b/blue_bringup/test/test_flake8.py @@ -12,14 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_flake8.main import main_with_errors import pytest +from ament_flake8.main import main_with_errors @pytest.mark.flake8 @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/blue_bringup/test/test_pep257.py b/blue_bringup/test/test_pep257.py index b234a384..4eddb46e 100644 --- a/blue_bringup/test/test_pep257.py +++ b/blue_bringup/test/test_pep257.py @@ -12,12 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -from ament_pep257.main import main import pytest +from ament_pep257.main import main @pytest.mark.linter @pytest.mark.pep257 def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" From 51b8400b7546d08ef767ff923ae16a94919dc902 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 16 May 2023 21:31:52 -0700 Subject: [PATCH 3/6] Added message interval client --- .devcontainer.json | 2 +- blue_bringup/config/blue.yaml | 2 + .../include/blue_control/base_controller.hpp | 8 ++ blue_control/src/base_controller.cpp | 105 +++++++++++++++--- blue_manager/blue_manager/manager.py | 4 +- 5 files changed, 103 insertions(+), 18 deletions(-) diff --git a/.devcontainer.json b/.devcontainer.json index d0ae4926..8124cbcf 100644 --- a/.devcontainer.json +++ b/.devcontainer.json @@ -4,7 +4,7 @@ "build": { "args": { "WORKSPACE": "${containerWorkspaceFolder}", - "ROS_DISTRO": "humble" + "ROS_DISTRO": "rolling" } }, "remoteUser": "blue", diff --git a/blue_bringup/config/blue.yaml b/blue_bringup/config/blue.yaml index 1425054e..1af0ebc8 100644 --- a/blue_bringup/config/blue.yaml +++ b/blue_bringup/config/blue.yaml @@ -23,6 +23,8 @@ ismc: 0.06, -0.06, 0.06, -0.06, -0.218, -0.218, 0.218, 0.218, 0.06, 0.06, -0.06, -0.06, 0.120, -0.120, 0.120, -0.120, -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0] + msg_ids: [31, 32] + msg_rates: [100.0, 100.0] mavros: ros__parameters: diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp index ef19e6ed..1ac34eb6 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/base_controller.hpp @@ -29,6 +29,7 @@ #include "blue_dynamics/hydrodynamics.hpp" #include "blue_dynamics/thruster_dynamics.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" +#include "mavros_msgs/srv/message_interval.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/battery_state.hpp" @@ -124,6 +125,9 @@ class BaseController : public rclcpp::Node std::shared_ptr request, std::shared_ptr response); + void setMessageRates(const std::vector & msg_ids, const std::vector & rates); + void setMessageRate(int64_t msg_id, float rate); + bool armed_; // Publishers @@ -135,9 +139,13 @@ class BaseController : public rclcpp::Node // Timers rclcpp::TimerBase::SharedPtr control_loop_timer_; + rclcpp::TimerBase::SharedPtr set_message_rate_timer_; // Services rclcpp::Service::SharedPtr arm_srv_; + + // Service clients + rclcpp::Client::SharedPtr set_msg_interval_client_; }; } // namespace blue::control diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/base_controller.cpp index 6249caec..79c7b144 100644 --- a/blue_control/src/base_controller.cpp +++ b/blue_control/src/base_controller.cpp @@ -46,27 +46,29 @@ BaseController::BaseController(const std::string & node_name) this->declare_parameter("mass", 11.5); this->declare_parameter("buoyancy", 112.80); this->declare_parameter("weight", 114.80); - this->declare_parameter("inertia_tensor_coeff", std::vector{0.16, 0.16, 0.16}); + this->declare_parameter("inertia_tensor_coeff", std::vector({0.16, 0.16, 0.16})); this->declare_parameter( - "added_mass_coeff", std::vector{-5.50, -12.70, -14.60, -0.12, -0.12, -0.12}); + "added_mass_coeff", std::vector({-5.50, -12.70, -14.60, -0.12, -0.12, -0.12})); this->declare_parameter( - "linear_damping_coeff", std::vector{-4.03, -6.22, -5.18, -0.07, -0.07, -0.07}); + "linear_damping_coeff", std::vector({-4.03, -6.22, -5.18, -0.07, -0.07, -0.07})); this->declare_parameter( - "quadratic_damping_coeff", std::vector{-18.18, -21.66, -36.99, -1.55, -1.55, -1.55}); - this->declare_parameter("center_of_gravity", std::vector{0.0, 0.0, 0.0}); - this->declare_parameter("center_of_buoyancy", std::vector{0.0, 0.0, 0.0}); - this->declare_parameter("ocean_current", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + "quadratic_damping_coeff", std::vector({-18.18, -21.66, -36.99, -1.55, -1.55, -1.55})); + this->declare_parameter("center_of_gravity", std::vector({0.0, 0.0, 0.0})); + this->declare_parameter("center_of_buoyancy", std::vector({0.0, 0.0, 0.0})); + this->declare_parameter("ocean_current", std::vector({0.0, 0.0, 0.0, 0.0, 0.0, 0.0})); this->declare_parameter("num_thrusters", 8); + this->declare_parameter("msg_ids", std::vector({31, 32})); + this->declare_parameter("msg_rates", std::vector({100, 100})); // I'm so sorry for this // You can blame the ROS devs for not supporting nested arrays for parameters this->declare_parameter( - "tcm", std::vector{0.707, 0.707, -0.707, -0.707, 0.0, 0.0, 0.0, 0.0, - -0.707, 0.707, -0.707, 0.707, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, -1.0, 1.0, 1.0, -1.0, - 0.06, -0.06, 0.06, -0.06, -0.218, -0.218, 0.218, 0.218, - 0.06, 0.06, -0.06, -0.06, 0.120, -0.120, 0.120, -0.120, - -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0}); + "tcm", std::vector({0.707, 0.707, -0.707, -0.707, 0.0, 0.0, 0.0, 0.0, + -0.707, 0.707, -0.707, 0.707, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, -1.0, 1.0, 1.0, -1.0, + 0.06, -0.06, 0.06, -0.06, -0.218, -0.218, 0.218, 0.218, + 0.06, 0.06, -0.06, -0.06, 0.120, -0.120, 0.120, -0.120, + -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0})); // Get the parameter values const double mass = this->get_parameter("mass").as_double(); @@ -100,6 +102,7 @@ BaseController::BaseController(const std::string & node_name) blue::dynamics::RestoringForces(weight, buoyancy, center_of_buoyancy, center_of_gravity), blue::dynamics::CurrentEffects(ocean_current)); + // Setup the ROS things rc_override_pub_ = this->create_publisher("mavros/rc/override", 1); @@ -118,10 +121,38 @@ BaseController::BaseController(const std::string & node_name) armControllerCb(request, response); }); + set_msg_interval_client_ = + this->create_client("/mavros/set_message_interval"); + + // Wait for the service to be available + while (!set_msg_interval_client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO( // NOLINT + this->get_logger(), "Waiting for %s...", set_msg_interval_client_->get_service_name()); + } + + // Get the message IDs to request from the autopilot and the rates at which they should be sent + const std::vector msg_ids = this->get_parameter("msg_ids").as_integer_array(); + + // ROS returns a std::vector, but we want an std::vector for the message rates + const std::vector msg_rates_double = this->get_parameter("msg_rates").as_double_array(); + const std::vector msg_rates(msg_rates_double.begin(), msg_rates_double.end()); + + // A 2nd GCS (e.g., QGC) might change message rates on launch, e.g.,: + // https://discuss.bluerobotics.com/t/qgroundcontrol-stream-rates/12204 + // Set up a timer to periodically set message rates + set_message_rate_timer_ = this->create_wall_timer( + std::chrono::seconds(10), + [this, msg_ids, msg_rates]() -> void { setMessageRates(msg_ids, msg_rates); }); + // Run the controller at a rate of 200 Hz // ArduSub only runs at a rate of 100 Hz, but we want to make sure to run the controller at // a faster rate than the autopilot - control_loop_timer_ = this->create_wall_timer(std::chrono::milliseconds(5), [this]() { + control_loop_timer_ = this->create_wall_timer(std::chrono::milliseconds(5), [this]() -> void { if (armed_) { rc_override_pub_->publish(update()); } @@ -147,4 +178,50 @@ void BaseController::armControllerCb( } } +void BaseController::setMessageRates( + const std::vector & msg_ids, const std::vector & rates) +{ + // Check that the message IDs and rates are the same length + if (msg_ids.size() != rates.size()) { + RCLCPP_ERROR( + this->get_logger(), + "Message IDs and rates must be the same length. Message IDs: %ld, rates: %ld", msg_ids.size(), + rates.size()); + return; + } + + // Set the message rates + for (size_t i = 0; i < msg_ids.size(); i++) { + setMessageRate(msg_ids[i], rates[i]); + } +} + +void BaseController::setMessageRate(int64_t msg_id, float rate) +{ + auto request = std::make_shared(); + + request->message_id = msg_id; + request->message_rate = rate; + + RCLCPP_DEBUG( + get_logger(), "Set message rate for %d to %g hz", request->message_id, request->message_rate); + + auto future = set_msg_interval_client_->async_send_request( + request, + [this, &request](rclcpp::Client::SharedFuture future) { + try { + auto response = future.get(); + + if (!response->success) { + RCLCPP_ERROR( + this->get_logger(), "Failed to set message rate for %d to %g hz", request->message_id, + request->message_rate); + } + } + catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to set message rate: %s", e.what()); + } + }); +} + } // namespace blue::control diff --git a/blue_manager/blue_manager/manager.py b/blue_manager/blue_manager/manager.py index efb40f12..451c697e 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -29,8 +29,6 @@ from rclpy.node import Node from std_srvs.srv import SetBool -# TODO(evan): Add message requests for pose, attitude, and battery - class Manager(Node): """Provides an interface between custom controllers and the BlueROV2.""" @@ -78,7 +76,7 @@ def __init__(self) -> None: # Service clients def wait_for_client(client) -> None: while not client.wait_for_service(timeout_sec=1.0): - ... + self.get_logger().info(f"Waiting for {client.srv_name}...") self.set_param_srv_client = self.create_client( SetParameters, From c417c43f3118f3f65e2afe2b3fcdba9acac7913c Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 16 May 2023 21:33:54 -0700 Subject: [PATCH 4/6] Added vision pose plugin --- blue_bringup/config/blue.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/blue_bringup/config/blue.yaml b/blue_bringup/config/blue.yaml index 1af0ebc8..3ab21c78 100644 --- a/blue_bringup/config/blue.yaml +++ b/blue_bringup/config/blue.yaml @@ -36,6 +36,7 @@ mavros: - local_position - rc_io - param + - vision_pose mavros_node: ros__parameters: From 6493d467d064c60a2ad2e5608d569e9f20afadeb Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 16 May 2023 21:37:18 -0700 Subject: [PATCH 5/6] Added docstrings --- .../include/blue_control/base_controller.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp index 1ac34eb6..2b68550e 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/base_controller.hpp @@ -125,7 +125,23 @@ class BaseController : public rclcpp::Node std::shared_ptr request, std::shared_ptr response); + /** + * @brief Set custom MAVLink message rates. + * + * @note This is inspired by the Orca4 project: + * https://github.com/clydemcqueen/orca4/tree/main + * + * @param msg_ids The message IDs to set the rates for. + * @param rates The frequencies that the FCU should send the messages at. + */ void setMessageRates(const std::vector & msg_ids, const std::vector & rates); + + /** + * @brief Set the rate of a MAVLink message. + * + * @param msg_id The message ID to set the rate for. + * @param rate The frequency that the FCU should send the message at. + */ void setMessageRate(int64_t msg_id, float rate); bool armed_; From 9e07fea95c7c7a3096cecc5a97a3071419ba0ca3 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 16 May 2023 22:09:43 -0700 Subject: [PATCH 6/6] Fixed pr comments --- blue_bringup/launch/blue.launch.py | 36 +++++++++--------------------- 1 file changed, 11 insertions(+), 25 deletions(-) diff --git a/blue_bringup/launch/blue.launch.py b/blue_bringup/launch/blue.launch.py index 6d3851d3..5c1cbde7 100644 --- a/blue_bringup/launch/blue.launch.py +++ b/blue_bringup/launch/blue.launch.py @@ -50,20 +50,20 @@ def generate_launch_description() -> LaunchDescription: ), ] + config_filepath = PathJoinSubstitution( + [ + FindPackageShare("blue_bringup"), + "config", + LaunchConfiguration("config"), + ] + ) + nodes = [ Node( package="mavros", executable="mavros_node", output="screen", - parameters=[ - PathJoinSubstitution( - [ - FindPackageShare("blue_bringup"), - "config", - LaunchConfiguration("config"), - ] - ), - ], + parameters=[config_filepath], ), ] @@ -75,15 +75,7 @@ def generate_launch_description() -> LaunchDescription: [FindPackageShare("blue_manager"), "manager.launch.py"] ) ), - launch_arguments={ - "config_filepath": PathJoinSubstitution( - [ - FindPackageShare("blue_bringup"), - "config", - LaunchConfiguration("config"), - ] - ), - }.items(), + launch_arguments={"config_filepath": config_filepath}.items(), ), IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -92,13 +84,7 @@ def generate_launch_description() -> LaunchDescription: ) ), launch_arguments={ - "config_filepath": PathJoinSubstitution( - [ - FindPackageShare("blue_bringup"), - "config", - LaunchConfiguration("config"), - ] - ), + "config_filepath": config_filepath, "controller": LaunchConfiguration("controller"), }.items(), ),