diff --git a/.devcontainer.json b/.devcontainer.json index 461b19f5..8124cbcf 100644 --- a/.devcontainer.json +++ b/.devcontainer.json @@ -4,10 +4,10 @@ "build": { "args": { "WORKSPACE": "${containerWorkspaceFolder}", - "ROS_DISTRO": "humble" + "ROS_DISTRO": "rolling" } }, - "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..3ab21c78 --- /dev/null +++ b/blue_bringup/config/blue.yaml @@ -0,0 +1,50 @@ +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] + msg_ids: [31, 32] + msg_rates: [100.0, 100.0] + +mavros: + ros__parameters: + system_id: 255 + plugin_allowlist: + - sys_status + - command + - imu + - local_position + - rc_io + - param + - vision_pose + +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..5c1cbde7 --- /dev/null +++ b/blue_bringup/launch/blue.launch.py @@ -0,0 +1,93 @@ +# 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.actions import Node +from launch_ros.substitutions import FindPackageShare + + +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"], + ), + ] + + config_filepath = PathJoinSubstitution( + [ + FindPackageShare("blue_bringup"), + "config", + LaunchConfiguration("config"), + ] + ) + + nodes = [ + Node( + package="mavros", + executable="mavros_node", + output="screen", + parameters=[config_filepath], + ), + ] + + # Declare additional launch files to run + includes = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("blue_manager"), "manager.launch.py"] + ) + ), + launch_arguments={"config_filepath": config_filepath}.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("blue_control"), "launch", "control.launch.py"] + ) + ), + launch_arguments={ + "config_filepath": config_filepath, + "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..8f18fa4b --- /dev/null +++ b/blue_bringup/test/test_copyright.py @@ -0,0 +1,27 @@ +# 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. + +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.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..f494570f --- /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. + +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) diff --git a/blue_bringup/test/test_pep257.py b/blue_bringup/test/test_pep257.py new file mode 100644 index 00000000..4eddb46e --- /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. + +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" 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/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/base_controller.hpp index ef19e6ed..2b68550e 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,25 @@ 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_; // Publishers @@ -135,9 +155,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/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_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 a7bc2b17..451c697e 100644 --- a/blue_manager/blue_manager/manager.py +++ b/blue_manager/blue_manager/manager.py @@ -76,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, 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,