diff --git a/.clang-format b/.clang-format index 369d0cd..6033928 100644 --- a/.clang-format +++ b/.clang-format @@ -12,8 +12,11 @@ AlignAfterOpenBracket: AlwaysBreak ConstructorInitializerIndentWidth: 0 ContinuationIndentWidth: 2 DerivePointerAlignment: false +AllowAllParametersOfDeclarationOnNextLine: false PointerAlignment: Middle PackConstructorInitializers: Never +BinPackArguments: false +BinPackParameters: false # Configure brace wrapping cases BreakBeforeBraces: Custom diff --git a/.clang-tidy b/.clang-tidy index 9528f15..3666bb2 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -13,8 +13,6 @@ Checks: > -google-readability-namespace-comments, -google-runtime-references, -misc-non-private-member-variables-in-classes, - -modernize-return-braced-init-list, - -modernize-use-trailing-return-type, -readability-braces-around-statements, -readability-identifier-length, -readability-magic-numbers, diff --git a/.docker/Dockerfile b/.devcontainer/Dockerfile similarity index 61% rename from .docker/Dockerfile rename to .devcontainer/Dockerfile index 9aa921d..494472e 100644 --- a/.docker/Dockerfile +++ b/.devcontainer/Dockerfile @@ -1,5 +1,5 @@ ARG ROS_DISTRO=iron -FROM ros:$ROS_DISTRO-ros-base as ci +FROM ros:$ROS_DISTRO-ros-base ENV DEBIAN_FRONTEND=noninteractive WORKDIR /root/ws_ros @@ -15,32 +15,17 @@ RUN apt-get -q update \ git \ sudo \ clang \ - clang-format-14 \ - clang-tidy \ - clang-tools \ python3-pip \ python3-dev \ + python3-venv \ apt-utils \ software-properties-common \ && apt-get autoremove -y \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* -# Install all ROS dependencies needed for CI -RUN vcs import src < src/$PROJECT_NAME/ros2.repos \ - && apt-get -q update \ - && apt-get -q -y upgrade \ - && rosdep update \ - && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \ - && rm -rf src \ - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* - -FROM ci as desktop - -# Configure a new non-root user -ARG USERNAME=ros +# Configure the ubuntu non-root user +ARG USERNAME=ubuntu ARG USER_UID=1000 ARG USER_GID=$USER_UID @@ -51,50 +36,57 @@ RUN groupadd --gid $USER_GID $USERNAME \ && usermod -a -G dialout $USERNAME \ && echo "source /usr/share/bash-completion/completions/git" >> /home/$USERNAME/.bashrc -# Switch to the non-root user +# Switch to the ubuntu user USER $USERNAME ENV USER=$USERNAME -ENV DEBIAN_FRONTEND=noninteractive ENV USER_WORKSPACE=/home/$USERNAME/ws_ros WORKDIR $USER_WORKSPACE COPY --chown=$USER_UID:$USER_GID . src/$PROJECT_NAME +# Create a new virtual environment for Python +ENV VIRTUAL_ENV=$USER_WORKSPACE/.venv/$PROJECT_NAME +RUN python3 -m venv --system-site-packages $VIRTUAL_ENV \ + && echo "source ${VIRTUAL_ENV}/bin/activate" >> /home/$USERNAME/.bashrc \ + && touch .venv/COLCON_IGNORE \ + && echo "\n# Ensure colcon is run in the venv\nalias colcon='python3 -m colcon'" >> /home/$USERNAME/.bashrc +ENV PATH="$VIRTUAL_ENV/bin:$PATH" + # Install all ROS dependencies +RUN vcs import src < src/$PROJECT_NAME/ros2.repos WORKDIR $USER_WORKSPACE -RUN vcs import src < src/$PROJECT_NAME/ros2.repos \ - && sudo apt-get -q update \ +RUN sudo apt-get -q update \ && sudo apt-get -q -y upgrade \ && rosdep update \ - && rosdep install -y --from-paths src --ignore-src -r --rosdistro ${ROS_DISTRO} \ + && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} \ && sudo apt-get autoremove -y \ && sudo apt-get clean -y \ && sudo rm -rf /var/lib/apt/lists/* RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \ && colcon build \ - && sudo sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$USER_WORKSPACE/setup.sh#g" /ros_entrypoint.sh \ && echo "source ${USER_WORKSPACE}/install/setup.bash" >> /home/$USERNAME/.bashrc \ && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc -FROM desktop as desktop-nvidia +WORKDIR $USER_WORKSPACE + +# Install debugging/linting Python packages +RUN pip install \ + pre-commit \ + mypy -# Install NVIDIA software -RUN sudo apt-get update \ +# Install debugging/linting C++ packages +RUN sudo apt-get -q update \ && sudo apt-get -q -y upgrade \ - && sudo apt-get install -y -qq --no-install-recommends \ - libglvnd0 \ - libgl1 \ - libglx0 \ - libegl1 \ - libxext6 \ - libx11-6 \ + && sudo apt-get install -y \ + clang-format-14 \ + clang-tidy \ + clang-tools \ && sudo apt-get autoremove -y \ && sudo apt-get clean -y \ && sudo rm -rf /var/lib/apt/lists/* -# Env vars for the nvidia-container-runtime. -ENV NVIDIA_VISIBLE_DEVICES all -ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute -ENV QT_X11_NO_MITSHM 1 +# Disable the setuputils installation warning +# This prevents us from needing to pin the setuputils version (which doesn't always work) +ENV PYTHONWARNINGS="ignore" diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..5535d12 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,26 @@ +{ + "name": "ROS 2 Dev Container", + "dockerFile": "Dockerfile", + "context": "../", + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/ws_ros/src/auv_controllers,type=bind", + "workspaceFolder": "/home/ubuntu/ws_ros/src/auv_controllers", + "remoteUser": "ubuntu", + "customizations": { + "vscode": { + "extensions": [ + "ms-azuretools.vscode-docker", + "ms-python.python", + "njpwerner.autodocstring", + "ms-vscode.cpptools", + "redhat.vscode-xml", + "redhat.vscode-yaml", + "smilerobotics.urdf", + "DavidAnson.vscode-markdownlint", + "esbenp.prettier-vscode", + "xaver.clang-format", + "charliermarsh.ruff", + "ms-vscode.cmake-tools" + ] + } + } +} diff --git a/.devcontainer/nouveau/Dockerfile b/.devcontainer/nouveau/Dockerfile deleted file mode 100644 index 9fd93e2..0000000 --- a/.devcontainer/nouveau/Dockerfile +++ /dev/null @@ -1,27 +0,0 @@ -FROM ghcr.io/robotic-decision-making-lab/auv_controllers:iron-desktop - -# Install ROS dependencies -# This is done in a previous stage, but we include it again here in case anyone wants to -# add new dependencies during development -ENV USERNAME=ros -ENV USER_WORKSPACE=/home/$USERNAME/ws_ros -WORKDIR $USER_WORKSPACE - -COPY --chown=$USER_UID:$USER_GID . src/$PROJECT_NAME - -RUN sudo apt-get -q update \ - && sudo apt-get -q -y upgrade \ - && rosdep update \ - && rosdep install -y --from-paths . --ignore-src -r --rosdistro ${ROS_DISTRO} \ - && sudo apt-get autoremove -y \ - && sudo apt-get clean -y \ - && sudo rm -rf /var/lib/apt/lists/* - -# Install debugging/linting Python packages -COPY --chown=$USER_UID:$USER_GID requirements-dev.txt . -RUN python3 -m pip install -r requirements-dev.txt \ - && rm -rf requirements-dev.txt - -# Disable the setuputils installation warning -# This prevents us from needing to pin the setuputils version (which doesn't always work) -ENV PYTHONWARNINGS="ignore" diff --git a/.devcontainer/nouveau/devcontainer.json b/.devcontainer/nouveau/devcontainer.json deleted file mode 100644 index 84f36b8..0000000 --- a/.devcontainer/nouveau/devcontainer.json +++ /dev/null @@ -1,42 +0,0 @@ -{ - "name": "ROS 2 Dev Container", - "dockerFile": "Dockerfile", - "context": "../..", - "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros/ws_ros/src/auv_controllers,type=bind", - "workspaceFolder": "/home/ros/ws_ros/src/auv_controllers", - "remoteUser": "ros", - "runArgs": [ - "--network=host", - "--cap-add=SYS_PTRACE", - "--security-opt=seccomp:unconfined", - "--security-opt=apparmor:unconfined", - "--volume=/dev:/dev", - "--privileged", - "--volume=/run/user/1000:/run/user/1000" - ], - "containerEnv": { - "DISPLAY": "${localEnv:DISPLAY}", - "WAYLAND_DISPLAY": "${localEnv:WAYLAND_DISPLAY}", - "XDG_RUNTIME_DIR": "${localEnv:XDG_RUNTIME_DIR}", - "PULSE_SERVER": "${localEnv:PULSE_SERVER}" - }, - "customizations": { - "vscode": { - "extensions": [ - "ms-azuretools.vscode-docker", - "ms-python.python", - "njpwerner.autodocstring", - "ms-vscode.cpptools", - "redhat.vscode-xml", - "redhat.vscode-yaml", - "smilerobotics.urdf", - "DavidAnson.vscode-markdownlint", - "esbenp.prettier-vscode", - "xaver.clang-format", - "charliermarsh.ruff", - "ms-python.black-formatter", - "josetr.cmake-language-support-vscode" - ] - } - } -} diff --git a/.devcontainer/nvidia/Dockerfile b/.devcontainer/nvidia/Dockerfile deleted file mode 100644 index 0f2fc2e..0000000 --- a/.devcontainer/nvidia/Dockerfile +++ /dev/null @@ -1,27 +0,0 @@ -FROM ghcr.io/robotic-decision-making-lab/auv_controllers:iron-desktop-nvidia - -# Install ROS dependencies -# This is done in a previous stage, but we include it again here in case anyone wants to -# add new dependencies during development -ENV USERNAME=ros -ENV USER_WORKSPACE=/home/$USERNAME/ws_ros -WORKDIR $USER_WORKSPACE - -COPY --chown=$USER_UID:$USER_GID . src/$PROJECT_NAME - -RUN sudo apt-get -q update \ - && sudo apt-get -q -y upgrade \ - && rosdep update \ - && rosdep install -y --from-paths . --ignore-src -r --rosdistro ${ROS_DISTRO} \ - && sudo apt-get autoremove -y \ - && sudo apt-get clean -y \ - && sudo rm -rf /var/lib/apt/lists/* - -# Install debugging/linting Python packages -COPY --chown=$USER_UID:$USER_GID requirements-dev.txt . -RUN python3 -m pip install -r requirements-dev.txt \ - && rm -rf requirements-dev.txt - -# Disable the setuputils installation warning -# This prevents us from needing to pin the setuputils version (which doesn't always work) -ENV PYTHONWARNINGS="ignore" diff --git a/.devcontainer/nvidia/devcontainer.json b/.devcontainer/nvidia/devcontainer.json deleted file mode 100644 index 77ff2ef..0000000 --- a/.devcontainer/nvidia/devcontainer.json +++ /dev/null @@ -1,46 +0,0 @@ -{ - "name": "ROS 2 NVIDIA Dev Container", - "dockerFile": "Dockerfile", - "context": "../..", - "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros/ws_ros/src/auv_controllers,type=bind", - "workspaceFolder": "/home/ros/ws_ros/src/auv_controllers", - "remoteUser": "ros", - "runArgs": [ - "--network=host", - "--cap-add=SYS_PTRACE", - "--security-opt=seccomp:unconfined", - "--security-opt=apparmor:unconfined", - "--volume=/dev:/dev", - "--privileged", - "--volume=/tmp/.X11-unix:/tmp/.X11-unix", - "--volume=/mnt/wslg:/mnt/wslg", - "--gpus=all" - ], - "containerEnv": { - "DISPLAY": "${localEnv:DISPLAY}", - "WAYLAND_DISPLAY": "${localEnv:WAYLAND_DISPLAY}", - "XDG_RUNTIME_DIR": "${localEnv:XDG_RUNTIME_DIR}", - "PULSE_SERVER": "${localEnv:PULSE_SERVER}", - "LIBGL_ALWAYS_SOFTWARE": "1", - "QT_X11_NO_MITSHM": "1" - }, - "customizations": { - "vscode": { - "extensions": [ - "ms-azuretools.vscode-docker", - "ms-python.python", - "njpwerner.autodocstring", - "ms-vscode.cpptools", - "redhat.vscode-xml", - "redhat.vscode-yaml", - "smilerobotics.urdf", - "DavidAnson.vscode-markdownlint", - "esbenp.prettier-vscode", - "xaver.clang-format", - "charliermarsh.ruff", - "ms-python.black-formatter", - "josetr.cmake-language-support-vscode" - ] - } - } -} diff --git a/.dockerignore b/.dockerignore index ceebdb8..dbb98a2 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,11 +1,13 @@ # ignore everything + * # Except the following -!requirements-dev.txt + !auv_controllers !velocity_controllers !thruster_allocation_matrix_controller !thruster_controllers !auv_control_msgs +!auv_control_demos !ros2.repos diff --git a/.github/mergify.yml b/.github/mergify.yml new file mode 100644 index 0000000..186c695 --- /dev/null +++ b/.github/mergify.yml @@ -0,0 +1,44 @@ +pull_request_rules: + - name: backport to iron at reviewers' discretion + conditions: + - base=main + - "label=backport-iron" + actions: + backport: + branches: + - iron + + - name: backport to humble at reviewers' discretion + conditions: + - base=main + - "label=backport-humble" + actions: + backport: + branches: + - humble + + - name: backport to jazzy at reviewers' discretion + conditions: + - base=main + - "label=backport-jazzy" + actions: + backport: + branches: + - jazzy + + - name: ask to resolve conflict + conditions: + - conflict + - author!=mergify[bot] + actions: + comment: + message: This pull request is in conflict. Could you fix it @{{author}}? + + - name: development targets main branch + conditions: + - base!=main + - author!=mergify[bot] + actions: + comment: + message: | + Please target the `main` branch for development, we will backport the changes to {{base}} for you if approved and if they don't break API. diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e99e019..c58acd5 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -19,29 +19,17 @@ jobs: fail-fast: false matrix: env: - - IMAGE: iron-ci - ROS_DISTRO: iron + - ROS_DISTRO: rolling steps: - name: Checkout repository uses: actions/checkout@v4 - with: - submodules: recursive - - - name: Log into registry - uses: docker/login-action@v3.2.0 - with: - registry: ghcr.io - username: ${{ github.actor }} - password: ${{ secrets.GITHUB_TOKEN }} - name: Run ROS Industrial CI uses: ros-industrial/industrial_ci@master env: - DOCKER_IMAGE: ghcr.io/robotic-decision-making-lab/auv_controllers:${{ matrix.env.IMAGE }} + ROS_DISTRO: ${{ matrix.env.ROS_DISTRO }} + CXXFLAGS: -Wall -Wextra -Wpedantic + CLANG_TIDY: true UPSTREAM_WORKSPACE: ros2.repos AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src - CXXFLAGS: >- - -Wall -Wextra -Wpedantic -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls - CC: ${{ env.CLANG_TIDY && 'clang' }} - CXX: ${{ env.CLANG_TIDY && 'clang++' }} diff --git a/.github/workflows/docker.yml b/.github/workflows/docker.yml deleted file mode 100644 index a9107ec..0000000 --- a/.github/workflows/docker.yml +++ /dev/null @@ -1,138 +0,0 @@ -name: Docker - -on: - push: - branches: - - iron - pull_request: - paths: - - .docker/** - - .github/workflows/docker.yaml - workflow_dispatch: - -env: - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'robotic-decision-making-lab/auv_controllers') }} - -jobs: - ci: - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [iron] - runs-on: ubuntu-latest - permissions: - packages: write - contents: read - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Log into registry - if: env.PUSH == 'true' - uses: docker/login-action@v3.2.0 - with: - registry: ghcr.io - username: ${{ github.actor }} - password: ${{ secrets.GITHUB_TOKEN }} - - - name: Extract Docker metadata - if: env.PUSH == 'true' - id: meta - uses: docker/metadata-action@v5.5.1 - with: - images: ghcr.io/${{ github.repository }} - tags: | - type=raw,value=${{ matrix.ROS_DISTRO }}-${{ github.job }} - - - name: Build and push Docker image - uses: docker/build-push-action@v6.7.0 - with: - context: . - file: .docker/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} - target: ${{ github.job }} - tags: ${{ steps.meta.outputs.tags }} - labels: ${{ steps.meta.outputs.labels }} - push: ${{ env.PUSH }} - - desktop: - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [iron] - runs-on: ubuntu-latest - permissions: - packages: write - contents: read - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Log into registry - if: env.PUSH == 'true' - uses: docker/login-action@v3.2.0 - with: - registry: ghcr.io - username: ${{ github.actor }} - password: ${{ secrets.GITHUB_TOKEN }} - - - name: Extract Docker metadata - if: env.PUSH == 'true' - id: meta - uses: docker/metadata-action@v5.5.1 - with: - images: ghcr.io/${{ github.repository }} - tags: | - type=raw,value=${{ matrix.ROS_DISTRO }}-${{ github.job }} - - - name: Build and push Docker image - uses: docker/build-push-action@v6.7.0 - with: - context: . - file: .docker/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} - target: ${{ github.job }} - tags: ${{ steps.meta.outputs.tags }} - labels: ${{ steps.meta.outputs.labels }} - push: ${{ env.PUSH }} - - desktop-nvidia: - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [iron] - runs-on: ubuntu-latest - permissions: - packages: write - contents: read - steps: - - name: Checkout repository - uses: actions/checkout@v4 - - - name: Log into registry - if: env.PUSH == 'true' - uses: docker/login-action@v3.2.0 - with: - registry: ghcr.io - username: ${{ github.actor }} - password: ${{ secrets.GITHUB_TOKEN }} - - - name: Extract Docker metadata - if: env.PUSH == 'true' - id: meta - uses: docker/metadata-action@v5.5.1 - with: - images: ghcr.io/${{ github.repository }} - tags: | - type=raw,value=${{ matrix.ROS_DISTRO }}-${{ github.job }} - - - name: Build and push Docker image - uses: docker/build-push-action@v6.7.0 - with: - context: . - file: .docker/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} - target: ${{ github.job }} - tags: ${{ steps.meta.outputs.tags }} - labels: ${{ steps.meta.outputs.labels }} - push: ${{ env.PUSH }} diff --git a/.gitignore b/.gitignore index 6cd0460..83e6a5a 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,6 @@ build/ install/ log/ + +.mypy_cache/ +.ruff_cache/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7cbbab9..0521495 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -9,17 +9,17 @@ repos: rev: v2.2.4 hooks: - id: codespell - args: ["--write-changes"] - - repo: local + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v18.1.8 hooks: - - id: clang-format - name: clang-format - description: Format files with ClangFormat. - entry: clang-format-14 - language: system - files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-fallback-style=Google', '-i'] + - id: clang-format + types_or: [c++, c] + + - repo: https://github.com/BlankSpruce/gersemi + rev: 0.13.5 + hooks: + - id: gersemi - repo: https://github.com/pre-commit/pre-commit-hooks rev: v4.4.0 @@ -31,9 +31,6 @@ repos: - id: check-yaml - id: check-xml - id: check-merge-conflict - - id: check-symlinks - - id: debug-statements - - id: destroyed-symlinks - id: detect-private-key - id: end-of-file-fixer - id: mixed-line-ending diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3c60578..d7d7f5d 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -6,13 +6,13 @@ "${workspaceFolder}/**", "/opt/ros/iron/include/**", "/usr/include/eigen3/**", - "/home/ros/ws_ros/**" + "/home/${USER}/ws_ros/install/**" ], "defines": [], - "compilerPath": "/usr/bin/gcc", + "compilerPath": "/usr/bin/clang", "cStandard": "c99", "cppStandard": "c++20", - "intelliSenseMode": "linux-gcc-x64" + "intelliSenseMode": "linux-clang-x64" } ], "version": 4 diff --git a/.vscode/settings.json b/.vscode/settings.json index ab0bdaf..a76cd24 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -32,11 +32,14 @@ "/opt/ros/iron/local/lib/python3.10/dist-packages/", "${workspaceFolder}/install/" ], + "python.defaultInterpreterPath": "${workspaceFolder}/.venv/waterlinked_dvl/bin/python", "C_Cpp.default.intelliSenseMode": "linux-gcc-x86", "C_Cpp.clang_format_fallbackStyle": "Google", "C_Cpp.codeAnalysis.clangTidy.enabled": true, "C_Cpp.codeAnalysis.clangTidy.codeAction.formatFixes": true, - "clang-format.executable": "/usr/bin/clang-format-14", + "clang-format.executable": "/usr/bin/clang-format-18", + "xml.format.maxLineWidth": 120, + "xml.format.splitAttributes": "alignWithFirstAttr", "[cpp]": { "editor.rulers": [120], "editor.tabSize": 2, @@ -50,7 +53,7 @@ "source.fixAll": "explicit", "source.organizeImports": "explicit" }, - "editor.defaultFormatter": "ms-python.black-formatter" + "editor.defaultFormatter": "charliermarsh.ruff" }, "[dockerfile]": { "editor.quickSuggestions": { @@ -69,9 +72,13 @@ "editor.rulers": [80], "editor.defaultFormatter": "DavidAnson.vscode-markdownlint" }, + "[yaml]": { + "editor.rulers": [80] + }, "search.exclude": { "**/build": true, "**/install": true, "**/log": true - } + }, + "cmake.ignoreCMakeListsMissing": true } diff --git a/auv_control_demos/CMakeLists.txt b/auv_control_demos/CMakeLists.txt index 00e708f..467f4ff 100644 --- a/auv_control_demos/CMakeLists.txt +++ b/auv_control_demos/CMakeLists.txt @@ -1,25 +1,22 @@ cmake_minimum_required(VERSION 3.8) project(auv_control_demos) -set(THIS_PACKAGE_INCLUDE_DEPENDS - rclpy - ament_cmake -) +set(THIS_PACKAGE_INCLUDE_DEPENDS rclpy ament_cmake) find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) + find_package(${Dependency} REQUIRED) endforeach() install( - DIRECTORY individual_controller/launch - individual_controller/config - individual_controller/xacro - chained_controllers/launch - chained_controllers/config - chained_controllers/xacro - - DESTINATION share/${PROJECT_NAME} + DIRECTORY + individual_controller/launch + individual_controller/config + individual_controller/xacro + chained_controllers/launch + chained_controllers/config + chained_controllers/xacro + DESTINATION share/${PROJECT_NAME} ) ament_package() diff --git a/auv_control_demos/chained_controllers/launch/chaining.launch.py b/auv_control_demos/chained_controllers/launch/chaining.launch.py index 5a1f113..de4104b 100644 --- a/auv_control_demos/chained_controllers/launch/chaining.launch.py +++ b/auv_control_demos/chained_controllers/launch/chaining.launch.py @@ -114,12 +114,17 @@ def generate_launch_description(): ) nodes = [ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ), Node( package="controller_manager", executable="ros2_control_node", output="both", parameters=[ - robot_description, PathJoinSubstitution( [ FindPackageShare("auv_control_demos"), @@ -128,6 +133,9 @@ def generate_launch_description(): ] ), ], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], ), *delay_thruster_spawners, delay_tam_controller_spawner_after_thruster_controller_spawners, diff --git a/auv_control_demos/chained_controllers/xacro/chained_config.xacro b/auv_control_demos/chained_controllers/xacro/chained_config.xacro index 83c24ac..1e17ed6 100644 --- a/auv_control_demos/chained_controllers/xacro/chained_config.xacro +++ b/auv_control_demos/chained_controllers/xacro/chained_config.xacro @@ -1,5 +1,6 @@ - + diff --git a/auv_control_demos/chained_controllers/xacro/chained_ros2_control.xacro b/auv_control_demos/chained_controllers/xacro/chained_ros2_control.xacro index 00855be..95696d9 100644 --- a/auv_control_demos/chained_controllers/xacro/chained_ros2_control.xacro +++ b/auv_control_demos/chained_controllers/xacro/chained_ros2_control.xacro @@ -1,6 +1,7 @@ - + mock_components/GenericSystem false diff --git a/auv_control_demos/chained_controllers/xacro/chained_urdf.xacro b/auv_control_demos/chained_controllers/xacro/chained_urdf.xacro index 7a05a70..d663453 100644 --- a/auv_control_demos/chained_controllers/xacro/chained_urdf.xacro +++ b/auv_control_demos/chained_controllers/xacro/chained_urdf.xacro @@ -1,130 +1,170 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/auv_control_demos/individual_controller/launch/individual.launch.py b/auv_control_demos/individual_controller/launch/individual.launch.py index 9c56bdb..5d709f5 100644 --- a/auv_control_demos/individual_controller/launch/individual.launch.py +++ b/auv_control_demos/individual_controller/launch/individual.launch.py @@ -47,12 +47,17 @@ def generate_launch_description(): return LaunchDescription( [ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ), Node( package="controller_manager", executable="ros2_control_node", output="both", parameters=[ - robot_description, PathJoinSubstitution( [ FindPackageShare("auv_control_demos"), @@ -61,6 +66,9 @@ def generate_launch_description(): ] ), ], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], ), Node( package="controller_manager", diff --git a/auv_control_demos/individual_controller/xacro/individual_config.xacro b/auv_control_demos/individual_controller/xacro/individual_config.xacro index 4104e8a..0706330 100644 --- a/auv_control_demos/individual_controller/xacro/individual_config.xacro +++ b/auv_control_demos/individual_controller/xacro/individual_config.xacro @@ -1,5 +1,6 @@ - + diff --git a/auv_control_demos/individual_controller/xacro/individual_ros2_control.xacro b/auv_control_demos/individual_controller/xacro/individual_ros2_control.xacro index 3472c9a..bc6aef6 100644 --- a/auv_control_demos/individual_controller/xacro/individual_ros2_control.xacro +++ b/auv_control_demos/individual_controller/xacro/individual_ros2_control.xacro @@ -1,6 +1,8 @@ - - + + mock_components/GenericSystem diff --git a/auv_control_demos/individual_controller/xacro/individual_urdf.xacro b/auv_control_demos/individual_controller/xacro/individual_urdf.xacro index f529c09..1267197 100644 --- a/auv_control_demos/individual_controller/xacro/individual_urdf.xacro +++ b/auv_control_demos/individual_controller/xacro/individual_urdf.xacro @@ -1,41 +1,49 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml index 222d0db..e43b061 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -3,7 +3,8 @@ auv_control_demos 0.0.1 - Example package that includes demos for using auv_controllers in individual and chained modes + Example package that includes demos for using auv_controllers in individual and + chained modes Colin Mitchell Everardo Gonzalez @@ -21,6 +22,9 @@ ament_cmake + robot_state_publisher + xacro + ament_lint_auto ament_lint_common diff --git a/requirements-dev.txt b/requirements-dev.txt deleted file mode 100644 index a3bd924..0000000 --- a/requirements-dev.txt +++ /dev/null @@ -1,3 +0,0 @@ -# Development tools not required for project installation -pre-commit -mypy diff --git a/thruster_allocation_matrix_controller/CMakeLists.txt b/thruster_allocation_matrix_controller/CMakeLists.txt index 6c7b45d..e2173c1 100644 --- a/thruster_allocation_matrix_controller/CMakeLists.txt +++ b/thruster_allocation_matrix_controller/CMakeLists.txt @@ -2,87 +2,81 @@ cmake_minimum_required(VERSION 3.8) project(thruster_allocation_matrix_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic) endif() +include(GNUInstallDirs) + set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - eigen3_cmake_module - Eigen3 - geometry_msgs - auv_control_msgs - control_msgs - hydrodynamics + rclcpp + rclcpp_lifecycle + controller_interface + hardware_interface + realtime_tools + eigen3_cmake_module + Eigen3 + geometry_msgs + auv_control_msgs + control_msgs + hydrodynamics ) -find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) + find_package(${Dependency} REQUIRED) endforeach() +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(pluginlib REQUIRED) + generate_parameter_library(thruster_allocation_matrix_controller_parameters src/thruster_allocation_matrix_controller_parameters.yaml ) -add_library(thruster_allocation_matrix_controller SHARED - src/thruster_allocation_matrix_controller.cpp -) +add_library(thruster_allocation_matrix_controller SHARED) -target_include_directories(thruster_allocation_matrix_controller - PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIR} - PRIVATE - ${PROJECT_SOURCE_DIR}/src +target_sources( + thruster_allocation_matrix_controller + PRIVATE src/thruster_allocation_matrix_controller.cpp ) -target_compile_features(thruster_allocation_matrix_controller PUBLIC cxx_std_17) -target_link_libraries(thruster_allocation_matrix_controller - PUBLIC - ${rclcpp_LIBRARIES} - thruster_allocation_matrix_controller_parameters +target_include_directories( + thruster_allocation_matrix_controller + PUBLIC + $ + $ ) - -# Use dllexport instead of dllimport -target_compile_definitions(thruster_allocation_matrix_controller PRIVATE "THRUSTER_ALLOCATION_MATRIX_CONTROLLER_BUILDING_DLL") -ament_target_dependencies(thruster_allocation_matrix_controller - PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_link_libraries( + thruster_allocation_matrix_controller + PRIVATE thruster_allocation_matrix_controller_parameters ) +ament_target_dependencies(thruster_allocation_matrix_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_features(thruster_allocation_matrix_controller PUBLIC cxx_std_20) pluginlib_export_plugin_description_file(controller_interface thruster_allocation_matrix_controller.xml) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + ament_lint_auto_find_test_dependencies() endif() install( - DIRECTORY include/ - DESTINATION include/thruster_allocation_matrix_controller + DIRECTORY include/ + DESTINATION include/thruster_allocation_matrix_controller ) install( - TARGETS - thruster_allocation_matrix_controller - thruster_allocation_matrix_controller_parameters - EXPORT - export_thruster_allocation_matrix_controller - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include + TARGETS + thruster_allocation_matrix_controller + thruster_allocation_matrix_controller_parameters + EXPORT export_thruster_allocation_matrix_controller + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} ) ament_export_targets(export_thruster_allocation_matrix_controller HAS_LIBRARY_TARGET) diff --git a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp b/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp index b1e0aac..38b9fc1 100644 --- a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp +++ b/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.hpp @@ -37,7 +37,6 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" -#include "thruster_allocation_matrix_controller/visibility_control.h" // auto-generated by generate_parameter_library #include "thruster_allocation_matrix_controller_parameters.hpp" @@ -45,52 +44,36 @@ namespace thruster_allocation_matrix_controller { -/** - * @brief Controller used to convert desired wrenches into thruster forces using a thruster allocation matrix. - */ +/// Controller used to convert wrench values into thruster forces using a thruster allocation matrix. class ThrusterAllocationMatrixController : public controller_interface::ChainableControllerInterface { public: - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC ThrusterAllocationMatrixController() = default; - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_init() override; + auto on_init() -> controller_interface::CallbackReturn override; - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; + auto command_interface_configuration() const -> controller_interface::InterfaceConfiguration override; - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + auto state_interface_configuration() const -> controller_interface::InterfaceConfiguration override; - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + auto on_configure(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + auto on_activate(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + auto update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type override; - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC - controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC - controller_interface::return_type update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC - bool on_set_chained_mode(bool chained_mode) override; + auto on_set_chained_mode(bool chained_mode) -> bool override; protected: - std::vector on_export_reference_interfaces() override; + auto on_export_reference_interfaces() -> std::vector override; - controller_interface::return_type update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type override; void update_parameters(); - controller_interface::CallbackReturn configure_parameters(); + auto configure_parameters() -> controller_interface::CallbackReturn; realtime_tools::RealtimeBuffer> reference_; std::shared_ptr> reference_sub_; @@ -104,10 +87,10 @@ class ThrusterAllocationMatrixController : public controller_interface::Chainabl std::vector thruster_names_; Eigen::MatrixXd tam_; - size_t num_thrusters_; + std::size_t num_thrusters_; private: - static constexpr size_t DOF = 6; + static constexpr std::size_t DOF = 6; std::array dof_names_{"x", "y", "z", "rx", "ry", "rz"}; }; diff --git a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/visibility_control.h b/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/visibility_control.h deleted file mode 100644 index 3be244e..0000000 --- a/thruster_allocation_matrix_controller/include/thruster_allocation_matrix_controller/visibility_control.h +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2024, Colin Mitchell, Everardo Gonzalez, Rakesh Vivekanandan -// -// 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. - -#ifndef THRUSTER_ALLOCATION_MATRIX_CONTROLLER__VISIBILITY_CONTROL_H_ -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_EXPORT __attribute__((dllexport)) -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_IMPORT __attribute__((dllimport)) -#else -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_EXPORT __declspec(dllexport) -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_IMPORT __declspec(dllimport) -#endif -#ifdef THRUSTER_ALLOCATION_MATRIX_CONTROLLER_BUILDING_DLL -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC THRUSTER_ALLOCATION_MATRIX_CONTROLLER_EXPORT -#else -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC THRUSTER_ALLOCATION_MATRIX_CONTROLLER_IMPORT -#endif -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC_TYPE THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_LOCAL -#else -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_EXPORT __attribute__((visibility("default"))) -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_IMPORT -#if __GNUC__ >= 4 -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC __attribute__((visibility("default"))) -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) -#else -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_LOCAL -#endif -#define THRUSTER_ALLOCATION_MATRIX_CONTROLLER_PUBLIC_TYPE -#endif - -#endif // THRUSTER_ALLOCATION_MATRIX_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index 5625cda..1a53e28 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,9 +3,8 @@ thruster_allocation_matrix_controller - 0.0.1 - Thruster allocation matrix controller used to convert wrench commands into thrust - commands + 0.0.2 + Thruster allocation matrix controller used to convert wrench commands into thrust commands Evan Palmer MIT diff --git a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp index 3f68d6b..32a1143 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp @@ -31,7 +31,7 @@ namespace thruster_allocation_matrix_controller namespace { -void reset_wrench_msg(std::shared_ptr wrench_msg) // NOLINT +auto reset_wrench_msg(std::shared_ptr wrench_msg) -> void // NOLINT { wrench_msg->force.x = std::numeric_limits::quiet_NaN(); wrench_msg->force.y = std::numeric_limits::quiet_NaN(); @@ -43,7 +43,7 @@ void reset_wrench_msg(std::shared_ptr wrench_msg) / } // namespace -controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_init() +auto ThrusterAllocationMatrixController::on_init() -> controller_interface::CallbackReturn { try { param_listener_ = std::make_shared(get_node()); @@ -57,7 +57,7 @@ controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_init return controller_interface::CallbackReturn::SUCCESS; } -void ThrusterAllocationMatrixController::update_parameters() +auto ThrusterAllocationMatrixController::update_parameters() -> void // NOLINT { if (!param_listener_->is_old(params_)) { return; @@ -66,7 +66,7 @@ void ThrusterAllocationMatrixController::update_parameters() params_ = param_listener_->get_params(); } -controller_interface::CallbackReturn ThrusterAllocationMatrixController::configure_parameters() +auto ThrusterAllocationMatrixController::configure_parameters() -> controller_interface::CallbackReturn { update_parameters(); @@ -75,22 +75,26 @@ controller_interface::CallbackReturn ThrusterAllocationMatrixController::configu // Make sure that the number prefixes (if provided) provided match the number of thrusters if (!params_.reference_controllers.empty() && params_.reference_controllers.size() != num_thrusters_) { - RCLCPP_ERROR( // NOLINT - get_node()->get_logger(), "Mismatched number of command interface prefixes and thrusters. Expected %ld, got %ld.", - num_thrusters_, params_.reference_controllers.size()); + RCLCPP_ERROR( + get_node()->get_logger(), + "Mismatched number of command interface prefixes and thrusters. Expected %ld, got %ld.", + num_thrusters_, + params_.reference_controllers.size()); return controller_interface::CallbackReturn::ERROR; } - const std::vector> vecs = {params_.tam.x, params_.tam.y, params_.tam.z, - params_.tam.rx, params_.tam.ry, params_.tam.rz}; + const std::vector> vecs = { + params_.tam.x, params_.tam.y, params_.tam.z, params_.tam.rx, params_.tam.ry, params_.tam.rz}; // Make sure that all of the rows are the same size for (const auto & vec : vecs) { - const size_t vec_size = vec.size(); + const std::size_t vec_size = vec.size(); if (vec_size != num_thrusters_) { - RCLCPP_ERROR( // NOLINT - get_node()->get_logger(), "Mismatched TAM row sizes. Expected %ld thrusters, got %ld.", num_thrusters_, + RCLCPP_ERROR( + get_node()->get_logger(), + "Mismatched TAM row sizes. Expected %ld thrusters, got %ld.", + num_thrusters_, vec_size); return controller_interface::CallbackReturn::ERROR; @@ -110,8 +114,8 @@ controller_interface::CallbackReturn ThrusterAllocationMatrixController::configu return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +auto ThrusterAllocationMatrixController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn { auto ret = configure_parameters(); if (ret != controller_interface::CallbackReturn::SUCCESS) { @@ -124,8 +128,11 @@ controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_conf command_interfaces_.reserve(num_thrusters_); reference_sub_ = get_node()->create_subscription( - "~/reference", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) { reference_.writeFromNonRT(msg); }); // NOLINT + "~/reference", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + reference_.writeFromNonRT(msg); + }); controller_state_pub_ = get_node()->create_publisher( "~/status", rclcpp::SystemDefaultsQoS()); @@ -143,36 +150,25 @@ controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_conf return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +auto ThrusterAllocationMatrixController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn { reset_wrench_msg(*(reference_.readFromNonRT())); reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn ThrusterAllocationMatrixController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - -bool ThrusterAllocationMatrixController::on_set_chained_mode(bool /*chained_mode*/) { return true; } +auto ThrusterAllocationMatrixController::on_set_chained_mode(bool /*chained_mode*/) -> bool { return true; } -controller_interface::InterfaceConfiguration ThrusterAllocationMatrixController::command_interface_configuration() const +auto ThrusterAllocationMatrixController::command_interface_configuration() const + -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration command_interfaces_configuration; command_interfaces_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_configuration.names.reserve(num_thrusters_); - for (size_t i = 0; i < num_thrusters_; ++i) { + for (std::size_t i = 0; i < num_thrusters_; ++i) { if (params_.reference_controllers.empty()) { command_interfaces_configuration.names.emplace_back(thruster_names_[i] + "/" + hardware_interface::HW_IF_EFFORT); } else { @@ -184,21 +180,23 @@ controller_interface::InterfaceConfiguration ThrusterAllocationMatrixController: return command_interfaces_configuration; } -controller_interface::InterfaceConfiguration ThrusterAllocationMatrixController::state_interface_configuration() const +auto ThrusterAllocationMatrixController::state_interface_configuration() const + -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration state_interface_configuration; state_interface_configuration.type = controller_interface::interface_configuration_type::NONE; return state_interface_configuration; } -std::vector ThrusterAllocationMatrixController::on_export_reference_interfaces() +auto ThrusterAllocationMatrixController::on_export_reference_interfaces() + -> std::vector { reference_interfaces_.resize(DOF, std::numeric_limits::quiet_NaN()); std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); - for (size_t i = 0; i < DOF; ++i) { + for (std::size_t i = 0; i < DOF; ++i) { reference_interfaces.emplace_back( get_node()->get_name(), dof_names_[i] + "/" + hardware_interface::HW_IF_EFFORT, &reference_interfaces_[i]); } @@ -206,16 +204,21 @@ std::vector ThrusterAllocationMatrixContro return reference_interfaces; } -controller_interface::return_type ThrusterAllocationMatrixController::update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +auto ThrusterAllocationMatrixController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) -> controller_interface::return_type { auto * current_reference = reference_.readFromNonRT(); - const std::vector wrench = {(*current_reference)->force.x, (*current_reference)->force.y, - (*current_reference)->force.z, (*current_reference)->torque.x, - (*current_reference)->torque.y, (*current_reference)->torque.z}; + const std::vector wrench = { + (*current_reference)->force.x, + (*current_reference)->force.y, + (*current_reference)->force.z, + (*current_reference)->torque.x, + (*current_reference)->torque.y, + (*current_reference)->torque.z}; - for (size_t i = 0; i < wrench.size(); ++i) { + for (std::size_t i = 0; i < wrench.size(); ++i) { if (!std::isnan(wrench[i])) { reference_interfaces_[i] = wrench[i]; } @@ -226,13 +229,14 @@ controller_interface::return_type ThrusterAllocationMatrixController::update_ref return controller_interface::return_type::OK; } -controller_interface::return_type ThrusterAllocationMatrixController::update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) +auto ThrusterAllocationMatrixController::update_and_write_commands( + const rclcpp::Time & time, + const rclcpp::Duration & period) -> controller_interface::return_type { const Eigen::Vector6d reference_wrench(reference_interfaces_.data()); const Eigen::VectorXd thrust(tam_.completeOrthogonalDecomposition().pseudoInverse() * reference_wrench); - for (size_t i = 0; i < num_thrusters_; i++) { + for (std::size_t i = 0; i < num_thrusters_; i++) { command_interfaces_[i].set_value(thrust[i]); } @@ -241,7 +245,7 @@ controller_interface::return_type ThrusterAllocationMatrixController::update_and rt_controller_state_pub_->msg_.time_step = period.seconds(); rt_controller_state_pub_->msg_.reference = reference_interfaces_; - for (size_t i = 0; i < num_thrusters_; i++) { + for (std::size_t i = 0; i < num_thrusters_; i++) { rt_controller_state_pub_->msg_.output[i] = command_interfaces_[i].get_value(); } diff --git a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller_parameters.yaml b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller_parameters.yaml index ebd50b6..ece4efa 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller_parameters.yaml +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller_parameters.yaml @@ -1,54 +1,51 @@ thruster_allocation_matrix_controller: - thrusters: { - type: string_array, - default_value: [], - read_only: true, - description: "The list of thruster names. These should be provided in the same order as the thruster allocation matrix.", - validation: { + thrusters: + type: string_array + default_value: [] + read_only: true + description: "The list of thruster names. These should be provided in the same order as the thruster allocation matrix." + validation: not_empty<>: null - } - } - reference_controllers: { - type: string_array, - default_value: [], - read_only: true, - description: "The names of the reference controllers. This can be used to configure command interfaces in chained mode, and should be provided in the same order as the thruster names.", - } - tam: { - x: { - type: double_array, - read_only: true, - default_value: [], + + reference_controllers: + type: string_array + default_value: [] + read_only: true + description: "The names of the reference controllers. This can be used to configure command interfaces in chained mode, and should be provided in the same order as the thruster names." + + tam: + x: + type: double_array + read_only: true + default_value: [] description: "Thruster allocation matrix row for translation along the x-axis." - }, - y: { - type: double_array, - read_only: true, - default_value: [], + + y: + type: double_array + read_only: true + default_value: [] description: "Thruster allocation matrix row for translation along the y-axis." - }, - z: { - type: double_array, - read_only: true, - default_value: [], + + z: + type: double_array + read_only: true + default_value: [] description: "Thruster allocation matrix row for translation along the z-axis." - }, - rx: { - type: double_array, - read_only: true, - default_value: [], + + rx: + type: double_array + read_only: true + default_value: [] description: "Thruster allocation matrix row for rotation about the x-axis." - }, - ry: { - type: double_array, - read_only: true, - default_value: [], + + ry: + type: double_array + read_only: true + default_value: [] description: "Thruster allocation matrix row for rotation about the y-axis." - }, - rz: { - type: double_array, - read_only: true, - default_value: [], + + rz: + type: double_array + read_only: true + default_value: [] description: "Trhuster allocation matrix row for rotation about the z-axis." - } - } diff --git a/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml b/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml index 9987c12..bcb5f7b 100644 --- a/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml +++ b/thruster_allocation_matrix_controller/thruster_allocation_matrix_controller.xml @@ -1,8 +1,8 @@ + type="thruster_allocation_matrix_controller::ThrusterAllocationMatrixController" + base_class_type="controller_interface::ChainableControllerInterface"> Thruster allocation matrix controller used to convert wrench commands into thrust commands diff --git a/thruster_controllers/CMakeLists.txt b/thruster_controllers/CMakeLists.txt index a294d1c..6bd4925 100644 --- a/thruster_controllers/CMakeLists.txt +++ b/thruster_controllers/CMakeLists.txt @@ -2,82 +2,72 @@ cmake_minimum_required(VERSION 3.8) project(thruster_controllers) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic) endif() +include(GNUInstallDirs) + set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_msgs - control_msgs + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + std_msgs + control_msgs ) -find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) + find_package(${Dependency} REQUIRED) endforeach() +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(pluginlib REQUIRED) + generate_parameter_library(polynomial_thrust_curve_controller_parameters src/polynomial_thrust_curve_controller_parameters.yaml ) -add_library(thruster_controllers SHARED - src/polynomial_thrust_curve_controller.cpp -) +add_library(thruster_controllers SHARED) -target_include_directories(thruster_controllers - PUBLIC - $ - $ - PRIVATE - ${PROJECT_SOURCE_DIR}/src +target_sources( + thruster_controllers + PRIVATE src/polynomial_thrust_curve_controller.cpp ) -target_compile_features(thruster_controllers PUBLIC cxx_std_17) -target_link_libraries(thruster_controllers - PUBLIC - ${rclcpp_LIBRARIES} - polynomial_thrust_curve_controller_parameters +target_include_directories( + thruster_controllers + PUBLIC + $ + $ ) - -# Use dllexport instead of dllimport -target_compile_definitions(thruster_controllers PRIVATE "THRUSTER_CONTROLLERS_BUILDING_DLL") -ament_target_dependencies(thruster_controllers - PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_link_libraries( + thruster_controllers + PUBLIC ${rclcpp_LIBRARIES} polynomial_thrust_curve_controller_parameters ) +ament_target_dependencies(thruster_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_features(thruster_controllers PUBLIC cxx_std_20) pluginlib_export_plugin_description_file(controller_interface thruster_controllers.xml) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + ament_lint_auto_find_test_dependencies() endif() -install( - DIRECTORY include/ - DESTINATION include/thruster_controllers -) +install(DIRECTORY include/ DESTINATION include/thruster_controllers) install( - TARGETS - thruster_controllers - polynomial_thrust_curve_controller_parameters - EXPORT - export_thruster_controllers - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include + TARGETS thruster_controllers polynomial_thrust_curve_controller_parameters + EXPORT export_thruster_controllers + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} ) ament_export_targets(export_thruster_controllers HAS_LIBRARY_TARGET) diff --git a/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp b/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp index ef16b15..e20dedd 100644 --- a/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp +++ b/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp @@ -33,7 +33,6 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_msgs/msg/float64.hpp" -#include "thruster_controllers/visibility_control.h" // auto-generated by generate_parameter_library #include "polynomial_thrust_curve_controller_parameters.hpp" @@ -41,52 +40,36 @@ namespace thruster_controllers { -/** - * @brief Controller used to convert thruster forces into PWM signals using a polynomial thrust curve. - */ +/// Controller used to convert thruster forces into PWM signals using a polynomial thrust curve. class PolynomialThrustCurveController : public controller_interface::ChainableControllerInterface { public: - THRUSTER_CONTROLLERS_PUBLIC PolynomialThrustCurveController() = default; - THRUSTER_CONTROLLERS_PUBLIC - controller_interface::CallbackReturn on_init() override; + auto on_init() -> controller_interface::CallbackReturn override; - THRUSTER_CONTROLLERS_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; + auto command_interface_configuration() const -> controller_interface::InterfaceConfiguration override; - THRUSTER_CONTROLLERS_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + auto state_interface_configuration() const -> controller_interface::InterfaceConfiguration override; - THRUSTER_CONTROLLERS_PUBLIC - controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + auto on_configure(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; - THRUSTER_CONTROLLERS_PUBLIC - controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + auto on_activate(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; - THRUSTER_CONTROLLERS_PUBLIC - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + auto update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type override; - THRUSTER_CONTROLLERS_PUBLIC - controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; - - THRUSTER_CONTROLLERS_PUBLIC - controller_interface::return_type update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - THRUSTER_CONTROLLERS_PUBLIC - bool on_set_chained_mode(bool chained_mode) override; + auto on_set_chained_mode(bool chained_mode) -> bool override; protected: - std::vector on_export_reference_interfaces() override; + auto on_export_reference_interfaces() -> std::vector override; - controller_interface::return_type update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type override; - void update_parameters(); + auto update_parameters() -> void; - controller_interface::CallbackReturn configure_parameters(); + auto configure_parameters() -> controller_interface::CallbackReturn; realtime_tools::RealtimeBuffer> reference_; std::shared_ptr> reference_sub_; diff --git a/thruster_controllers/include/thruster_controllers/visibility_control.h b/thruster_controllers/include/thruster_controllers/visibility_control.h deleted file mode 100644 index a1b97d0..0000000 --- a/thruster_controllers/include/thruster_controllers/visibility_control.h +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2024, 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. - -#ifndef THRUSTER_CONTROLLERS__VISIBILITY_CONTROL_H_ -#define THRUSTER_CONTROLLERS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define THRUSTER_CONTROLLERS_EXPORT __attribute__((dllexport)) -#define THRUSTER_CONTROLLERS_IMPORT __attribute__((dllimport)) -#else -#define THRUSTER_CONTROLLERS_EXPORT __declspec(dllexport) -#define THRUSTER_CONTROLLERS_IMPORT __declspec(dllimport) -#endif -#ifdef THRUSTER_CONTROLLERS_BUILDING_DLL -#define THRUSTER_CONTROLLERS_PUBLIC THRUSTER_CONTROLLERS_EXPORT -#else -#define THRUSTER_CONTROLLERS_PUBLIC THRUSTER_CONTROLLERS_IMPORT -#endif -#define THRUSTER_CONTROLLERS_PUBLIC_TYPE THRUSTER_CONTROLLERS_PUBLIC -#define THRUSTER_CONTROLLERS_LOCAL -#else -#define THRUSTER_CONTROLLERS_EXPORT __attribute__((visibility("default"))) -#define THRUSTER_CONTROLLERS_IMPORT -#if __GNUC__ >= 4 -#define THRUSTER_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) -#define THRUSTER_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) -#else -#define THRUSTER_CONTROLLERS_PUBLIC -#define THRUSTER_CONTROLLERS_LOCAL -#endif -#define THRUSTER_CONTROLLERS_PUBLIC_TYPE -#endif - -#endif // THRUSTER_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/thruster_controllers/src/polynomial_thrust_curve_controller.cpp b/thruster_controllers/src/polynomial_thrust_curve_controller.cpp index 9433fb8..7d32b60 100644 --- a/thruster_controllers/src/polynomial_thrust_curve_controller.cpp +++ b/thruster_controllers/src/polynomial_thrust_curve_controller.cpp @@ -22,6 +22,7 @@ #include #include +#include #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -31,10 +32,10 @@ namespace thruster_controllers namespace { -[[nodiscard]] int calculate_pwm_from_thrust_curve(double force, const std::vector & coefficients) +[[nodiscard]] auto calculate_pwm_from_thrust_curve(double force, const std::vector & coefficients) -> int { double pwm = 0.0; - for (size_t i = 0; i < coefficients.size(); ++i) { + for (std::size_t i = 0; i < coefficients.size(); ++i) { pwm += coefficients[i] * std::pow(force, i); } return static_cast(std::round(pwm)); @@ -42,7 +43,7 @@ namespace } // namespace -controller_interface::CallbackReturn PolynomialThrustCurveController::on_init() +auto PolynomialThrustCurveController::on_init() -> controller_interface::CallbackReturn { try { param_listener_ = std::make_shared(get_node()); @@ -56,7 +57,7 @@ controller_interface::CallbackReturn PolynomialThrustCurveController::on_init() return controller_interface::CallbackReturn::SUCCESS; } -void PolynomialThrustCurveController::update_parameters() // NOLINT +auto PolynomialThrustCurveController::update_parameters() -> void // NOLINT { if (!param_listener_->is_old(params_)) { return; @@ -65,15 +66,15 @@ void PolynomialThrustCurveController::update_parameters() // NOLINT params_ = param_listener_->get_params(); } -controller_interface::CallbackReturn PolynomialThrustCurveController::configure_parameters() +auto PolynomialThrustCurveController::configure_parameters() -> controller_interface::CallbackReturn { update_parameters(); thruster_name_ = params_.thruster; return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn PolynomialThrustCurveController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +auto PolynomialThrustCurveController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn { auto ret = configure_parameters(); if (ret != controller_interface::CallbackReturn::SUCCESS) { @@ -86,8 +87,9 @@ controller_interface::CallbackReturn PolynomialThrustCurveController::on_configu command_interfaces_.reserve(1); reference_sub_ = get_node()->create_subscription( - "~/reference", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) { reference_.writeFromNonRT(msg); }); // NOLINT + "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT + reference_.writeFromNonRT(msg); + }); controller_state_pub_ = get_node()->create_publisher("~/status", rclcpp::SystemDefaultsQoS()); @@ -102,29 +104,18 @@ controller_interface::CallbackReturn PolynomialThrustCurveController::on_configu return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn PolynomialThrustCurveController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +auto PolynomialThrustCurveController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn { (*reference_.readFromNonRT())->data = std::numeric_limits::quiet_NaN(); reference_interfaces_.assign(reference_interfaces_.size(), std::numeric_limits::quiet_NaN()); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn PolynomialThrustCurveController::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::CallbackReturn PolynomialThrustCurveController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - return controller_interface::CallbackReturn::SUCCESS; -} - -bool PolynomialThrustCurveController::on_set_chained_mode(bool /*chained_mode*/) { return true; } +auto PolynomialThrustCurveController::on_set_chained_mode(bool /*chained_mode*/) -> bool { return true; } -controller_interface::InterfaceConfiguration PolynomialThrustCurveController::command_interface_configuration() const +auto PolynomialThrustCurveController::command_interface_configuration() const + -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration command_interface_config; command_interface_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -135,14 +126,16 @@ controller_interface::InterfaceConfiguration PolynomialThrustCurveController::co return command_interface_config; } -controller_interface::InterfaceConfiguration PolynomialThrustCurveController::state_interface_configuration() const +auto PolynomialThrustCurveController::state_interface_configuration() const + -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration state_interface_config; state_interface_config.type = controller_interface::interface_configuration_type::NONE; return state_interface_config; } -std::vector PolynomialThrustCurveController::on_export_reference_interfaces() +auto PolynomialThrustCurveController::on_export_reference_interfaces() + -> std::vector { reference_interfaces_.resize(1, std::numeric_limits::quiet_NaN()); @@ -150,14 +143,16 @@ std::vector PolynomialThrustCurveControlle reference_interfaces.reserve(reference_interfaces_.size()); reference_interfaces.emplace_back( - get_node()->get_name(), thruster_name_ + "/" + hardware_interface::HW_IF_EFFORT, + get_node()->get_name(), + thruster_name_ + "/" + hardware_interface::HW_IF_EFFORT, &reference_interfaces_[0]); // NOLINT return reference_interfaces; } -controller_interface::return_type PolynomialThrustCurveController::update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +auto PolynomialThrustCurveController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) -> controller_interface::return_type { auto * current_reference = reference_.readFromNonRT(); reference_interfaces_[0] = (*current_reference)->data; @@ -166,8 +161,9 @@ controller_interface::return_type PolynomialThrustCurveController::update_refere return controller_interface::return_type::OK; } -controller_interface::return_type PolynomialThrustCurveController::update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) +auto PolynomialThrustCurveController::update_and_write_commands( + const rclcpp::Time & time, + const rclcpp::Duration & period) -> controller_interface::return_type { // Just for readability const auto reference = reference_interfaces_[0]; @@ -203,4 +199,5 @@ controller_interface::return_type PolynomialThrustCurveController::update_and_wr #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - thruster_controllers::PolynomialThrustCurveController, controller_interface::ChainableControllerInterface) + thruster_controllers::PolynomialThrustCurveController, + controller_interface::ChainableControllerInterface) diff --git a/thruster_controllers/src/polynomial_thrust_curve_controller_parameters.yaml b/thruster_controllers/src/polynomial_thrust_curve_controller_parameters.yaml index 0abb2b9..daebd02 100644 --- a/thruster_controllers/src/polynomial_thrust_curve_controller_parameters.yaml +++ b/thruster_controllers/src/polynomial_thrust_curve_controller_parameters.yaml @@ -1,49 +1,46 @@ polynomial_thrust_curve_controller: - thruster: { - type: string, - default_value: "", - read_only: true, - description: "The name of the thruster.", - validation: { + thruster: + type: string + default_value: "" + read_only: true + description: "The name of the thruster." + validation: not_empty<>: null - } - } - thrust_curve_coefficients: { - type: double_array, - default_value: [], - read_only: true, - description: "The thrust-to-PWM curve polynomial coefficients. These should be provided in the order of the lowest degree to the highest degree.", - validation: { + + thrust_curve_coefficients: + type: double_array + default_value: [] + read_only: true + description: "The thrust-to-PWM curve polynomial coefficients. These should be provided in the order of the lowest degree to the highest degree." + validation: not_empty<>: null - } - } - min_thrust: { - type: double, - default_value: 0.0, - read_only: true, - description: "The minimum thrust that can be produced by the thruster.", - } - max_thrust: { - type: double, - default_value: 0.0, - read_only: true, - description: "The maximum thrust that can be produced by the thruster.", - } - min_deadband_pwm: { - type: int, - default_value: 0, - read_only: true, + + min_thrust: + type: double + default_value: 0.0 + read_only: true + description: "The minimum thrust that can be produced by the thruster." + + max_thrust: + type: double + default_value: 0.0 + read_only: true + description: "The maximum thrust that can be produced by the thruster." + + min_deadband_pwm: + type: int + default_value: 0 + read_only: true description: "The minimum PWM value in the deadband range for the thruster." - } - max_deadband_pwm: { - type: int, - default_value: 0, - read_only: true, + + max_deadband_pwm: + type: int + default_value: 0 + read_only: true description: "The maximum PWM value in the deadband range for the thruster." - } - neutral_pwm: { - type: int, - default_value: 0, - read_only: true, + + neutral_pwm: + type: int + default_value: 0 + read_only: true description: "A safe PWM value that is known to apply zero thrust." - } diff --git a/velocity_controllers/CMakeLists.txt b/velocity_controllers/CMakeLists.txt index b0f8727..44357bd 100644 --- a/velocity_controllers/CMakeLists.txt +++ b/velocity_controllers/CMakeLists.txt @@ -2,89 +2,79 @@ cmake_minimum_required(VERSION 3.8) project(velocity_controllers) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic) endif() +include(GNUInstallDirs) + set(THIS_PACKAGE_INCLUDE_DEPENDS - controller_interface - generate_parameter_library - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - eigen3_cmake_module - Eigen3 - hydrodynamics - geometry_msgs - control_msgs - tf2_eigen - tf2 - tf2_ros + controller_interface + hardware_interface + rclcpp + rclcpp_lifecycle + realtime_tools + eigen3_cmake_module + Eigen3 + hydrodynamics + geometry_msgs + control_msgs + tf2_eigen + tf2 + tf2_ros ) -find_package(ament_cmake REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) + find_package(${Dependency} REQUIRED) endforeach() +find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) +find_package(pluginlib REQUIRED) + generate_parameter_library(integral_sliding_mode_controller_parameters src/integral_sliding_mode_controller_parameters.yaml ) -add_library(velocity_controllers SHARED - src/integral_sliding_mode_controller.cpp -) +add_library(velocity_controllers SHARED) -target_include_directories(velocity_controllers - PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIR} - PRIVATE - ${PROJECT_SOURCE_DIR}/src +target_sources( + velocity_controllers + PRIVATE src/integral_sliding_mode_controller.cpp ) -target_compile_features(velocity_controllers PUBLIC cxx_std_17) -target_link_libraries(velocity_controllers - PUBLIC - ${rclcpp_LIBRARIES} - integral_sliding_mode_controller_parameters +target_include_directories( + velocity_controllers + PUBLIC + $ + $ + PRIVATE ${PROJECT_SOURCE_DIR}/src ) - -# Use dllexport instead of dllimport -target_compile_definitions(velocity_controllers PRIVATE "VELOCITY_CONTROLLERS_BUILDING_DLL") -ament_target_dependencies(velocity_controllers - PUBLIC - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_link_libraries( + velocity_controllers + PRIVATE integral_sliding_mode_controller_parameters ) +ament_target_dependencies(velocity_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_compile_features(velocity_controllers PUBLIC cxx_std_20) pluginlib_export_plugin_description_file(controller_interface velocity_controllers.xml) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) + find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + ament_lint_auto_find_test_dependencies() endif() -install( - DIRECTORY include/ - DESTINATION include/velocity_controllers -) +install(DIRECTORY include/ DESTINATION include/velocity_controllers) install( - TARGETS - velocity_controllers - integral_sliding_mode_controller_parameters - EXPORT - export_velocity_controllers - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include + TARGETS velocity_controllers integral_sliding_mode_controller_parameters + EXPORT export_velocity_controllers + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} ) ament_export_targets(export_velocity_controllers HAS_LIBRARY_TARGET) diff --git a/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp b/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp index 9e4fe1e..9045fc2 100644 --- a/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp +++ b/velocity_controllers/include/velocity_controllers/integral_sliding_mode_controller.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -39,7 +40,6 @@ #include "realtime_tools/realtime_publisher.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "velocity_controllers/visibility_control.h" // auto-generated by generate_parameter_library #include "integral_sliding_mode_controller_parameters.hpp" @@ -47,53 +47,42 @@ namespace velocity_controllers { -/** - * @brief Integral sliding mode controller (ISMC) for velocity control of an autonomous underwater vehicle. - */ +/// Integral sliding mode controller (ISMC) for velocity control of an autonomous underwater vehicle. class IntegralSlidingModeController : public controller_interface::ChainableControllerInterface { public: - VELOCITY_CONTROLLERS_PUBLIC IntegralSlidingModeController() = default; - VELOCITY_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init() override; + auto on_init() -> controller_interface::CallbackReturn override; - VELOCITY_CONTROLLERS_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; + auto command_interface_configuration() const -> controller_interface::InterfaceConfiguration override; - VELOCITY_CONTROLLERS_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + auto state_interface_configuration() const -> controller_interface::InterfaceConfiguration override; - VELOCITY_CONTROLLERS_PUBLIC - controller_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + auto on_cleanup(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; - VELOCITY_CONTROLLERS_PUBLIC - controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + auto on_configure(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; - VELOCITY_CONTROLLERS_PUBLIC - controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + auto on_activate(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; - VELOCITY_CONTROLLERS_PUBLIC - controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + auto on_deactivate(const rclcpp_lifecycle::State & previous_state) -> controller_interface::CallbackReturn override; - VELOCITY_CONTROLLERS_PUBLIC - controller_interface::return_type update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + auto update_and_write_commands(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type override; - VELOCITY_CONTROLLERS_PUBLIC - bool on_set_chained_mode(bool chained_mode) override; + auto on_set_chained_mode(bool chained_mode) -> bool override; protected: - std::vector on_export_reference_interfaces() override; + auto on_export_reference_interfaces() -> std::vector override; - controller_interface::return_type update_reference_from_subscribers( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + auto update_reference_from_subscribers(const rclcpp::Time & time, const rclcpp::Duration & period) + -> controller_interface::return_type override; - controller_interface::return_type update_system_state_values(); + auto update_system_state_values() -> controller_interface::return_type; - void update_parameters(); + auto update_parameters() -> void; - controller_interface::CallbackReturn configure_parameters(); + auto configure_parameters() -> controller_interface::CallbackReturn; realtime_tools::RealtimeBuffer> reference_; std::shared_ptr> reference_sub_; @@ -131,7 +120,7 @@ class IntegralSlidingModeController : public controller_interface::ChainableCont private: // Can't mark an array of strings with constexpr, so we just keep it private - static constexpr size_t DOF = 6; + static constexpr std::size_t DOF = 6; std::array dof_names_{"x", "y", "z", "rx", "ry", "rz"}; }; diff --git a/velocity_controllers/include/velocity_controllers/visibility_control.h b/velocity_controllers/include/velocity_controllers/visibility_control.h deleted file mode 100644 index 9c4f952..0000000 --- a/velocity_controllers/include/velocity_controllers/visibility_control.h +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2024, 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. - -#ifndef VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_ -#define VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define VELOCITY_CONTROLLERS_EXPORT __attribute__((dllexport)) -#define VELOCITY_CONTROLLERS_IMPORT __attribute__((dllimport)) -#else -#define VELOCITY_CONTROLLERS_EXPORT __declspec(dllexport) -#define VELOCITY_CONTROLLERS_IMPORT __declspec(dllimport) -#endif -#ifdef VELOCITY_CONTROLLERS_BUILDING_DLL -#define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_EXPORT -#else -#define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_IMPORT -#endif -#define VELOCITY_CONTROLLERS_PUBLIC_TYPE VELOCITY_CONTROLLERS_PUBLIC -#define VELOCITY_CONTROLLERS_LOCAL -#else -#define VELOCITY_CONTROLLERS_EXPORT __attribute__((visibility("default"))) -#define VELOCITY_CONTROLLERS_IMPORT -#if __GNUC__ >= 4 -#define VELOCITY_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) -#define VELOCITY_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) -#else -#define VELOCITY_CONTROLLERS_PUBLIC -#define VELOCITY_CONTROLLERS_LOCAL -#endif -#define VELOCITY_CONTROLLERS_PUBLIC_TYPE -#endif - -#endif // VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_ diff --git a/velocity_controllers/package.xml b/velocity_controllers/package.xml index 5925723..e4fe0c8 100644 --- a/velocity_controllers/package.xml +++ b/velocity_controllers/package.xml @@ -29,6 +29,7 @@ control_msgs tf2_eigen hydrodynamics + geometry_msgs ament_lint_auto ament_lint_common diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp index 412ee2c..f0f02b6 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller.cpp +++ b/velocity_controllers/src/integral_sliding_mode_controller.cpp @@ -51,7 +51,7 @@ void reset_twist_msg(std::shared_ptr msg) // NOLINT } // namespace -controller_interface::CallbackReturn IntegralSlidingModeController::on_init() +auto IntegralSlidingModeController::on_init() -> controller_interface::CallbackReturn { try { param_listener_ = std::make_shared(get_node()); @@ -65,13 +65,14 @@ controller_interface::CallbackReturn IntegralSlidingModeController::on_init() return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::InterfaceConfiguration IntegralSlidingModeController::command_interface_configuration() const +auto IntegralSlidingModeController::command_interface_configuration() const + -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration command_interface_configuration; command_interface_configuration.type = controller_interface::interface_configuration_type::INDIVIDUAL; for (const auto & dof : dof_names_) { - if (params_.reference_controller.length() <= 0) { + if (params_.reference_controller.empty()) { command_interface_configuration.names.emplace_back(dof + "/" + hardware_interface::HW_IF_EFFORT); } else { command_interface_configuration.names.emplace_back( @@ -82,7 +83,8 @@ controller_interface::InterfaceConfiguration IntegralSlidingModeController::comm return command_interface_configuration; } -controller_interface::InterfaceConfiguration IntegralSlidingModeController::state_interface_configuration() const +auto IntegralSlidingModeController::state_interface_configuration() const + -> controller_interface::InterfaceConfiguration { controller_interface::InterfaceConfiguration state_interface_configuration; @@ -99,14 +101,15 @@ controller_interface::InterfaceConfiguration IntegralSlidingModeController::stat return state_interface_configuration; } -std::vector IntegralSlidingModeController::on_export_reference_interfaces() +auto IntegralSlidingModeController::on_export_reference_interfaces() + -> std::vector { reference_interfaces_.resize(DOF, std::numeric_limits::quiet_NaN()); std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); - for (size_t i = 0; i < DOF; ++i) { + for (std::size_t i = 0; i < DOF; ++i) { reference_interfaces.emplace_back( get_node()->get_name(), dof_names_[i] + "/" + hardware_interface::HW_IF_VELOCITY, &reference_interfaces_[i]); } @@ -114,13 +117,13 @@ std::vector IntegralSlidingModeController: return reference_interfaces; } -controller_interface::CallbackReturn IntegralSlidingModeController::on_cleanup( - const rclcpp_lifecycle::State & /*previous state*/) +auto IntegralSlidingModeController::on_cleanup(const rclcpp_lifecycle::State & /*previous state*/) + -> controller_interface::CallbackReturn { return controller_interface::CallbackReturn::SUCCESS; } -void IntegralSlidingModeController::update_parameters() +auto IntegralSlidingModeController::update_parameters() -> void { if (!param_listener_->is_old(params_)) { return; @@ -129,7 +132,7 @@ void IntegralSlidingModeController::update_parameters() params_ = param_listener_->get_params(); } -controller_interface::CallbackReturn IntegralSlidingModeController::configure_parameters() +auto IntegralSlidingModeController::configure_parameters() -> controller_interface::CallbackReturn { update_parameters(); @@ -158,14 +161,16 @@ controller_interface::CallbackReturn IntegralSlidingModeController::configure_pa // Move the damping and restoring forces because we only use them once damping_ = std::make_unique(std::move(linear_damping), std::move(quadratic_damping)); restoring_forces_ = std::make_unique( - params_.hydrodynamics.weight, params_.hydrodynamics.buoyancy, std::move(center_of_buoyancy), + params_.hydrodynamics.weight, + params_.hydrodynamics.buoyancy, + std::move(center_of_buoyancy), std::move(center_of_gravity)); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn IntegralSlidingModeController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +auto IntegralSlidingModeController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn { auto ret = configure_parameters(); if (ret != controller_interface::CallbackReturn::SUCCESS) { @@ -183,14 +188,20 @@ controller_interface::CallbackReturn IntegralSlidingModeController::on_configure system_state_values_.resize(DOF, std::numeric_limits::quiet_NaN()); reference_sub_ = get_node()->create_subscription( - "~/reference", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) { reference_.writeFromNonRT(msg); }); // NOLINT + "~/reference", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + reference_.writeFromNonRT(msg); + }); // NOLINT // If we aren't reading from the state interfaces, subscribe to the system state topic if (params_.use_external_measured_states) { system_state_sub_ = get_node()->create_subscription( - "~/system_state", rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) { system_state_.writeFromNonRT(msg); }); // NOLINT + "~/system_state", + rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) { // NOLINT + system_state_.writeFromNonRT(msg); + }); } // Configure the TF buffer and listener @@ -205,7 +216,7 @@ controller_interface::CallbackReturn IntegralSlidingModeController::on_configure rt_controller_state_pub_->lock(); rt_controller_state_pub_->msg_.dof_states.resize(DOF); - for (size_t i = 0; i < DOF; ++i) { + for (std::size_t i = 0; i < DOF; ++i) { rt_controller_state_pub_->msg_.dof_states[i].name = dof_names_[i]; } rt_controller_state_pub_->unlock(); @@ -213,8 +224,8 @@ controller_interface::CallbackReturn IntegralSlidingModeController::on_configure return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn IntegralSlidingModeController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +auto IntegralSlidingModeController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn { // Indicate that the next state update will be the first: this is used to set the error initial conditions first_update_ = true; @@ -233,24 +244,29 @@ controller_interface::CallbackReturn IntegralSlidingModeController::on_activate( return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn IntegralSlidingModeController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) +auto IntegralSlidingModeController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) + -> controller_interface::CallbackReturn { return controller_interface::CallbackReturn::SUCCESS; } -bool IntegralSlidingModeController::on_set_chained_mode(bool /*chained_mode*/) { return true; } +auto IntegralSlidingModeController::on_set_chained_mode(bool /*chained_mode*/) -> bool { return true; } -controller_interface::return_type IntegralSlidingModeController::update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +auto IntegralSlidingModeController::update_reference_from_subscribers( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) -> controller_interface::return_type { auto * current_reference = reference_.readFromNonRT(); - const std::vector reference = {(*current_reference)->linear.x, (*current_reference)->linear.y, - (*current_reference)->linear.z, (*current_reference)->angular.x, - (*current_reference)->angular.y, (*current_reference)->angular.z}; + const std::vector reference = { + (*current_reference)->linear.x, + (*current_reference)->linear.y, + (*current_reference)->linear.z, + (*current_reference)->angular.x, + (*current_reference)->angular.y, + (*current_reference)->angular.z}; - for (size_t i = 0; i < reference.size(); ++i) { + for (std::size_t i = 0; i < reference.size(); ++i) { if (!std::isnan(reference[i])) { reference_interfaces_[i] = reference[i]; } @@ -261,16 +277,20 @@ controller_interface::return_type IntegralSlidingModeController::update_referenc return controller_interface::return_type::OK; } -controller_interface::return_type IntegralSlidingModeController::update_system_state_values() +auto IntegralSlidingModeController::update_system_state_values() -> controller_interface::return_type { if (params_.use_external_measured_states) { auto * current_system_state = system_state_.readFromRT(); - const std::vector state = {(*current_system_state)->linear.x, (*current_system_state)->linear.y, - (*current_system_state)->linear.z, (*current_system_state)->angular.x, - (*current_system_state)->angular.y, (*current_system_state)->angular.z}; + const std::vector state = { + (*current_system_state)->linear.x, + (*current_system_state)->linear.y, + (*current_system_state)->linear.z, + (*current_system_state)->angular.x, + (*current_system_state)->angular.y, + (*current_system_state)->angular.z}; - for (size_t i = 0; i < state.size(); ++i) { + for (std::size_t i = 0; i < state.size(); ++i) { if (!std::isnan(state[i])) { system_state_values_[i] = state[i]; } @@ -278,7 +298,7 @@ controller_interface::return_type IntegralSlidingModeController::update_system_s reset_twist_msg(*current_system_state); } else { - for (size_t i = 0; i < system_state_values_.size(); ++i) { + for (std::size_t i = 0; i < system_state_values_.size(); ++i) { system_state_values_[i] = state_interfaces_[i].get_value(); } } @@ -286,8 +306,9 @@ controller_interface::return_type IntegralSlidingModeController::update_system_s return controller_interface::return_type::OK; } -controller_interface::return_type IntegralSlidingModeController::update_and_write_commands( - const rclcpp::Time & time, const rclcpp::Duration & period) +auto IntegralSlidingModeController::update_and_write_commands( + const rclcpp::Time & time, + const rclcpp::Duration & period) -> controller_interface::return_type { if (params_.enable_parameter_update_without_reactivation) { configure_parameters(); @@ -300,8 +321,11 @@ controller_interface::return_type IntegralSlidingModeController::update_and_writ std::vector velocity_error_values; velocity_error_values.reserve(DOF); std::transform( - reference_interfaces_.begin(), reference_interfaces_.end(), system_state_values_.begin(), - std::back_inserter(velocity_error_values), [](double reference, double state) { + reference_interfaces_.begin(), + reference_interfaces_.end(), + system_state_values_.begin(), + std::back_inserter(velocity_error_values), + [](double reference, double state) { return !std::isnan(reference) && !std::isnan(state) ? reference - state : std::numeric_limits::quiet_NaN(); }); @@ -310,8 +334,7 @@ controller_interface::return_type IntegralSlidingModeController::update_and_writ auto all_nan = std ::all_of(velocity_error_values.begin(), velocity_error_values.end(), [&](double i) { return std::isnan(i); }); if (all_nan) { - RCLCPP_DEBUG( // NOLINT - get_node()->get_logger(), "All reference and system state values are NaN. Skipping control update."); + RCLCPP_DEBUG(get_node()->get_logger(), "All reference and system state values are NaN. Skipping control update."); return controller_interface::return_type::OK; } @@ -334,18 +357,21 @@ controller_interface::return_type IntegralSlidingModeController::update_and_writ tf2::fromMsg(t.transform.rotation, *system_rotation_.readFromRT()); } catch (const tf2::TransformException & e) { - RCLCPP_DEBUG( // NOLINT - get_node()->get_logger(), "Could not transform %s to %s. Using latest available transform. %s", - inertial_frame_id_.c_str(), vehicle_frame_id_.c_str(), e.what()); + RCLCPP_DEBUG( + get_node()->get_logger(), + "Could not transform %s to %s. Using latest available transform. %s", + inertial_frame_id_.c_str(), + vehicle_frame_id_.c_str(), + e.what()); } // Calculate the computed torque control // Assume a feed-forward acceleration of 0 const Eigen::Vector6d tau0 = - inertia_->getMassMatrix() * (proportional_gain_ * velocity_error) + - coriolis_->calculateCoriolisMatrix(velocity_state) * velocity_state + - damping_->calculateDampingMatrix(velocity_state) * velocity_state + - restoring_forces_->calculateRestoringForcesVector(system_rotation_.readFromRT()->toRotationMatrix()); + inertia_->mass_matrix * (proportional_gain_ * velocity_error) + + coriolis_->calculate_coriolis_matrix(velocity_state) * velocity_state + + damping_->calculate_damping_matrix(velocity_state) * velocity_state + + restoring_forces_->calculate_restoring_forces_vector(system_rotation_.readFromRT()->toRotationMatrix()); // Calculate the sliding surface Eigen::Vector6d surface = @@ -362,13 +388,13 @@ controller_interface::return_type IntegralSlidingModeController::update_and_writ // Total control torque const Eigen::Vector6d tau = tau0 + tau1; - for (size_t i = 0; i < DOF; ++i) { + for (std::size_t i = 0; i < DOF; ++i) { command_interfaces_[i].set_value(tau[i]); } if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) { rt_controller_state_pub_->msg_.header.stamp = time; - for (size_t i = 0; i < DOF; ++i) { + for (std::size_t i = 0; i < DOF; ++i) { rt_controller_state_pub_->msg_.dof_states[i].reference = reference_interfaces_[i]; rt_controller_state_pub_->msg_.dof_states[i].feedback = system_state_values_[i]; rt_controller_state_pub_->msg_.dof_states[i].error = velocity_error_values[i]; @@ -386,4 +412,5 @@ controller_interface::return_type IntegralSlidingModeController::update_and_writ #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - velocity_controllers::IntegralSlidingModeController, controller_interface::ChainableControllerInterface) + velocity_controllers::IntegralSlidingModeController, + controller_interface::ChainableControllerInterface) diff --git a/velocity_controllers/src/integral_sliding_mode_controller_parameters.yaml b/velocity_controllers/src/integral_sliding_mode_controller_parameters.yaml index 1d96fe3..178a166 100644 --- a/velocity_controllers/src/integral_sliding_mode_controller_parameters.yaml +++ b/velocity_controllers/src/integral_sliding_mode_controller_parameters.yaml @@ -1,127 +1,112 @@ integral_sliding_mode_controller: - use_external_measured_states: { - type: bool, - read_only: true, - default_value: false, + use_external_measured_states: + type: bool + read_only: true + default_value: false description: "Use velocity measurements obtained from a topic instead of from state interfaces." - } - enable_parameter_update_without_reactivation: { - type: bool, - default_value: true, + + enable_parameter_update_without_reactivation: + type: bool + default_value: true description: "If enabled, the parameters will be dynamically updated while the controller is running." - } - reference_controller: { - type: string, - default_value: "", - read_only: true, + + reference_controller: + type: string + default_value: "" + read_only: true description: "The prefix of the reference controller to send command to. This can be used to configure command interfaces in chained mode." - } - tf: { - base_frame: { - type: string, - default_value: "base_link_fsd", - read_only: true, - description: "The name of the vehicle base frame.", - validation: { + + tf: + base_frame: + type: string + default_value: "base_link_fsd" + read_only: true + description: "The name of the vehicle base frame." + validation: not_empty<>: null - } - }, - odom_frame: { - type: string, - default_value: "odom_ned", + odom_frame: + type: string + default_value: "odom_ned" read_only: true, - description: "The name of the inertial frame.", - validation: { + description: "The name of the inertial frame." + validation: not_empty<>: null - } - } - } - gains: { - rho: { - type: double, - default_value: 0.0, + + gains: + rho: + type: double + default_value: 0.0 description: "The sliding mode gain. This adjusts how quickly the controller drives the system to the sliding surface." - }, - lambda: { - type: double, - default_value: 0.0, + + lambda: + type: double + default_value: 0.0 description: "The boundary thickness of the tanh function used to attenuate ISMC chatter." - }, - Kp: { - type: double_array, - default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - description: "The proportional gains for the controller provided in the order: x, y, z, rx, ry, rz.", - validation: { + + Kp: + type: double_array + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + description: "The proportional gains for the controller provided in the order: x, y, z, rx, ry, rz." + validation: fixed_size<>: 6 - } - } - } - hydrodynamics: { - mass: { - type: double, - default_value: 0.0, - description: "The mass of the vehicle.", - validation: { + + hydrodynamics: + mass: + type: double + default_value: 0.0 + description: "The mass of the vehicle." + validation: gt<>: [0.0] - } - }, - moments_of_inertia: { - type: double_array, - default_value: [0.0, 0.0, 0.0], - description: "The moments of inertia of the vehicle in the order: Ixx, Iyy, Izz.", - validation: { + + moments_of_inertia: + type: double_array + default_value: [0.0, 0.0, 0.0] + description: "The moments of inertia of the vehicle in the order: Ixx, Iyy, Izz." + validation: fixed_size<>: 3 - } - }, - added_mass: { - type: double_array, - default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - description: "The added mass coefficients of the vehicle in the order: Xdu, Ydv, Zdw, Kdp, Mdq, Ndr.", - validation: { + + added_mass: + type: double_array + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + description: "The added mass coefficients of the vehicle in the order: Xdu, Ydv, Zdw, Kdp, Mdq, Ndr." + validation: fixed_size<>: 6 - } - }, - weight: { - type: double, - default_value: 0.0, - description: "The weight of the vehicle.", - }, - buoyancy: { - type: double, - default_value: 0.0, + + weight: + type: double + default_value: 0.0 + description: "The weight of the vehicle." + + buoyancy: + type: double + default_value: 0.0 description: "The buoyancy of the vehicle." - }, - center_of_buoyancy: { - type: double_array, - default_value: [0.0, 0.0, 0.0], - description: "The center-of-buoyancy of the vehicle in the order: x, y, z.", - validation: { + + center_of_buoyancy: + type: double_array + default_value: [0.0, 0.0, 0.0] + description: "The center-of-buoyancy of the vehicle in the order: x, y, z." + validation: fixed_size<>: 3 - } - }, - center_of_gravity: { - type: double_array, - default_value: [0.0, 0.0, 0.0], - description: "The center-of-gravity of the vehicle in the order: x, y, z.", - validation: { + + center_of_gravity: + type: double_array + default_value: [0.0, 0.0, 0.0] + description: "The center-of-gravity of the vehicle in the order: x, y, z." + validation: fixed_size<>: 3 - } - }, - linear_damping: { - type: double_array, - default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - description: "The linear damping coefficients of the vehicle in the order: Xu, Yv, Zw, Kp, Mq, Nr.", - validation: { + + linear_damping: + type: double_array + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + description: "The linear damping coefficients of the vehicle in the order: Xu, Yv, Zw, Kp, Mq, Nr." + validation: fixed_size<>: 6 - } - }, - quadratic_damping: { - type: double_array, - default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - description: "The quadratic damping coefficients of the vehicle in the order: Xuu, Yvv, Zww, Kpp, Mqq, Nrr.", - validation: { + + quadratic_damping: + type: double_array + default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + description: "The quadratic damping coefficients of the vehicle in the order: Xuu, Yvv, Zww, Kpp, Mqq, Nrr." + validation: fixed_size<>: 6 - } - }, - }