diff --git a/.clang-tidy b/.clang-tidy index 3666bb2..e99cfef 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -9,6 +9,7 @@ Checks: > performance-*, portability-*, readability-*, + -portability-avoid-pragma-once, -google-readability-braces-around-statements, -google-readability-namespace-comments, -google-runtime-references, diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 3d244d1..433c4cf 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -63,7 +63,7 @@ RUN sudo apt-get -q update \ && sudo rm -rf /var/lib/apt/lists/* RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \ - && colcon build \ + && colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON \ && 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 @@ -74,12 +74,16 @@ RUN pip install \ pre-commit # Install debugging/linting C++ packages +# libstdc++-16-dev completes the gcc-16 toolchain that gets pulled in transitively +# without it, which otherwise leaves clang-tidy's GCC auto-detection unable to find +# standard library headers. RUN sudo apt-get -q update \ && sudo apt-get -q -y upgrade \ && sudo apt-get install -y \ - clang-format-18 \ + clang-format-22 \ clang-tidy \ clang-tools \ + libstdc++-16-dev \ && sudo apt-get autoremove -y \ && sudo apt-get clean -y \ && sudo rm -rf /var/lib/apt/lists/* diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 5535d12..2a246cb 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,26 +1,9 @@ { - "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" - ] + "workspaceMount": "source=${localWorkspaceFolder},target=/home/ubuntu/ws_ros/src/auv_controllers,type=bind", + "workspaceFolder": "/home/ubuntu/ws_ros/src/auv_controllers", + "remoteUser": "ubuntu", + "build": { + "dockerfile": "Dockerfile", + "context": "../" } - } } diff --git a/.dockerignore b/.dockerignore index a72133e..87f61c1 100644 --- a/.dockerignore +++ b/.dockerignore @@ -13,4 +13,7 @@ !topic_sensors !twist_controllers !whole_body_controllers +!trajectory_controllers +!controller_coordinator +!impedance_controller !ros2.repos diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d970275..0fcb821 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -30,6 +30,10 @@ jobs: ROS_DISTRO: ${{ matrix.env.ROS_DISTRO }} CXXFLAGS: -Wall -Wextra -Wpedantic -std=c++20 CLANG_TIDY: true + # Ubuntu's gcc-16 metapackage is pulled in transitively without a matching + # libstdc++-16-dev, which leaves clang-tidy's GCC auto-detection pointing at + # an incomplete toolchain and unable to find standard library headers. + ADDITIONAL_DEBS: libstdc++-16-dev UPSTREAM_WORKSPACE: ros2.repos AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src diff --git a/.gitignore b/.gitignore index 83e6a5a..cd651b6 100644 --- a/.gitignore +++ b/.gitignore @@ -3,5 +3,12 @@ build/ install/ log/ +# Python Cache .mypy_cache/ .ruff_cache/ + +# CMake Cache +.cache + +# Zed settings +.zed diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index 7ba0f97..0000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "configurations": [ - { - "name": "Linux", - "includePath": [ - "${workspaceFolder}/**", - "/opt/ros/rolling/include/**", - "/usr/include/eigen3/**", - "/home/${USER}/ws_ros/install/**" - ], - "defines": [], - "compilerPath": "/usr/bin/clang", - "cStandard": "c99", - "cppStandard": "c++23", - "intelliSenseMode": "linux-clang-x64" - } - ], - "version": 4 -} diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index bf272b8..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,86 +0,0 @@ -{ - "files.associations": { - "*.repos": "yaml", - "*.world": "xml", - "*.xacro": "xml", - "*.rviz": "yaml", - "*.config": "xml", - "*.sdf": "xml", - "limits": "cpp", - "format": "cpp" - }, - "terminal.integrated.defaultProfile.linux": "bash", - "terminal.integrated.profiles.linux": { - "bash": { - "path": "bash", - "icon": "terminal-bash", - "args": ["-i"] - } - }, - "files.insertFinalNewline": true, - "files.trimTrailingWhitespace": true, - "editor.formatOnSave": true, - "editor.tabSize": 2, - "editor.detectIndentation": false, - "autoDocstring.startOnNewLine": false, - "autoDocstring.docstringFormat": "google-notypes", - "python.autoComplete.extraPaths": [ - "/opt/ros/rolling/lib/python3.12/site-packages/", - "/opt/ros/rolling/local/lib/python3.12/dist-packages/", - "${workspaceFolder}/install/" - ], - "python.analysis.extraPaths": [ - "/opt/ros/rolling/lib/python3.12/site-packages/", - "/opt/ros/rolling/local/lib/python3.12/dist-packages/", - "${workspaceFolder}/install/" - ], - "python.defaultInterpreterPath": "${workspaceFolder}/.venv/auv_controllers/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-18", - "xml.format.maxLineWidth": 120, - "xml.format.splitAttributes": "alignWithFirstAttr", - "[cpp]": { - "editor.rulers": [120], - "editor.tabSize": 2, - "editor.defaultFormatter": "xaver.clang-format" - }, - "[python]": { - "editor.tabSize": 4, - "editor.rulers": [90], - "editor.formatOnSave": true, - "editor.codeActionsOnSave": { - "source.fixAll": "explicit", - "source.organizeImports": "explicit" - }, - "editor.defaultFormatter": "charliermarsh.ruff" - }, - "[dockerfile]": { - "editor.quickSuggestions": { - "strings": true - }, - "editor.defaultFormatter": "ms-azuretools.vscode-docker", - "editor.tabSize": 4 - }, - "[json]": { - "editor.defaultFormatter": "esbenp.prettier-vscode" - }, - "[xml]": { - "editor.defaultFormatter": "redhat.vscode-xml" - }, - "[markdown]": { - "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/.vscode/tasks.json b/.vscode/tasks.json deleted file mode 100644 index a1b4d03..0000000 --- a/.vscode/tasks.json +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2.0.0", - "tasks": [ - { - "label": "ROS 2: Build", - "detail": "Build the workspace using colcon", - "type": "shell", - "command": "colcon build --symlink-install", - "group": { - "kind": "build", - "isDefault": true - }, - "problemMatcher": ["$gcc"] - }, - { - "label": "ROS 2: Test", - "detail": "Run system tests using colcon", - "type": "shell", - "command": "colcon test && colcon test-result --all", - "group": { - "kind": "test", - "isDefault": true - } - }, - { - "label": "ROS 2: Source workspace", - "detail": "Source the ROS 2 workspace", - "type": "shell", - "command": "source install/setup.bash", - "problemMatcher": [] - }, - { - "label": "ROS 2: Create ament_cmake package", - "detail": "Create a new ROS 2 ament_cmake package", - "type": "shell", - "command": "ros2 pkg create --build-type ament_cmake --license ${input:license} ${input:packageName}", - "problemMatcher": [] - }, - { - "label": "ROS 2: Create ament_python package", - "detail": "Create a new ROS 2 ament_python package", - "type": "shell", - "command": "ros2 pkg create --build-type ament_python --license ${input:license} ${input:packageName}", - "problemMatcher": [] - } - ], - "inputs": [ - { - "id": "license", - "type": "pickString", - "description": "License", - "options": [ - "Apache-2.0", - "BSL-1.0", - "BSD-2.0", - "BSD-2-Clause", - "BSD-3-Clause", - "GPL-3.0-only", - "LGPL-3.0-only", - "MIT", - "MIT-0" - ] - }, - { - "id": "packageName", - "type": "promptString", - "description": "Package name" - } - ] -} diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md index 68ab2eb..2eaab9d 100644 --- a/auv_control_demos/CHANGELOG.md +++ b/auv_control_demos/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package auv_control_demos +## 0.5.1 (2026-08-14) + +- Declares the missing `rclpy` dependency + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/auv_control_demos/chained_controllers/launch/chaining.launch.py b/auv_control_demos/chained_controllers/launch/chaining.launch.py index ad4d79f..8388369 100644 --- a/auv_control_demos/chained_controllers/launch/chaining.launch.py +++ b/auv_control_demos/chained_controllers/launch/chaining.launch.py @@ -47,6 +47,14 @@ def generate_launch_description(): ) robot_description = {"robot_description": robot_description_content} + controller_params = PathJoinSubstitution( + [ + FindPackageShare("auv_control_demos"), + "config", + "chained_controllers.yaml", + ] + ) + velocity_controller_spawner = Node( package="controller_manager", executable="spawner", @@ -54,6 +62,8 @@ def generate_launch_description(): "adaptive_integral_terminal_sliding_mode_controller", "--controller-manager", ["", "controller_manager"], + "--param-file", + controller_params, ], ) @@ -65,6 +75,8 @@ def generate_launch_description(): f"thruster_{i + 1}_controller", "--controller-manager", ["", "controller_manager"], + "--param-file", + controller_params, ], ) for i in range(8) @@ -93,6 +105,8 @@ def generate_launch_description(): "thruster_allocation_matrix_controller", "--controller-manager", ["", "controller_manager"], + "--param-file", + controller_params, ], ) delay_tam_controller_spawner_after_thruster_controller_spawners = ( diff --git a/auv_control_demos/individual_controller/launch/individual.launch.py b/auv_control_demos/individual_controller/launch/individual.launch.py index 155ad85..a87bf1c 100644 --- a/auv_control_demos/individual_controller/launch/individual.launch.py +++ b/auv_control_demos/individual_controller/launch/individual.launch.py @@ -45,6 +45,14 @@ def generate_launch_description(): ) robot_description = {"robot_description": robot_description_content} + controller_params = PathJoinSubstitution( + [ + FindPackageShare("auv_control_demos"), + "config", + "individual_controller.yaml", + ] + ) + return LaunchDescription( [ Node( @@ -57,15 +65,7 @@ def generate_launch_description(): package="controller_manager", executable="ros2_control_node", output="both", - parameters=[ - PathJoinSubstitution( - [ - FindPackageShare("auv_control_demos"), - "config", - "individual_controller.yaml", - ] - ), - ], + parameters=[controller_params], remappings=[ ("/controller_manager/robot_description", "/robot_description"), ], @@ -77,6 +77,8 @@ def generate_launch_description(): "adaptive_integral_terminal_sliding_mode_controller", "--controller-manager", ["", "controller_manager"], + "--param-file", + controller_params, ], ), ] diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml index cfc2ab9..da4003d 100644 --- a/auv_control_demos/package.xml +++ b/auv_control_demos/package.xml @@ -3,7 +3,7 @@ auv_control_demos - 0.5.0 + 0.5.1 Example package that includes demos for using auv_controllers in individual and chained modes Evan Palmer @@ -19,6 +19,8 @@ ament_cmake + rclpy + robot_state_publisher xacro diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md index 9323142..37e41db 100644 --- a/auv_control_msgs/CHANGELOG.md +++ b/auv_control_msgs/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package auv_control_msgs +## 0.5.1 (2026-08-14) + +- Declares the missing `builtin_interfaces` dependency + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml index 8ef6663..1b3b335 100644 --- a/auv_control_msgs/package.xml +++ b/auv_control_msgs/package.xml @@ -3,7 +3,7 @@ auv_control_msgs - 0.5.0 + 0.5.1 Custom messages for AUV controllers Evan Palmer @@ -21,6 +21,7 @@ std_msgs geometry_msgs trajectory_msgs + builtin_interfaces rosidl_default_runtime rosidl_interface_packages diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md index a45a4f2..4019d8a 100644 --- a/auv_controllers/CHANGELOG.md +++ b/auv_controllers/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package auv_controllers +## 0.5.1 (2026-08-14) + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml index 6d16bb2..f00d941 100644 --- a/auv_controllers/package.xml +++ b/auv_controllers/package.xml @@ -3,7 +3,7 @@ auv_controllers - 0.5.0 + 0.5.1 Meta package for auv_controllers Evan Palmer diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md index bf0af74..faeb0c8 100644 --- a/controller_common/CHANGELOG.md +++ b/controller_common/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package controller_common +## 0.5.1 (2026-08-14) + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/controller_common/package.xml b/controller_common/package.xml index 25ca26c..dadbb1c 100644 --- a/controller_common/package.xml +++ b/controller_common/package.xml @@ -3,7 +3,7 @@ controller_common - 0.5.0 + 0.5.1 Common interfaces for controllers used in this project Evan Palmer diff --git a/controller_common/src/common.cpp b/controller_common/src/common.cpp index 266badc..5903b80 100644 --- a/controller_common/src/common.cpp +++ b/controller_common/src/common.cpp @@ -22,7 +22,6 @@ #include #include -#include namespace common { @@ -193,17 +192,17 @@ auto calculate_error(const std::vector & reference, const std::vector & vec) -> bool { - return std::ranges::any_of(vec, [](double x) { return std::isnan(x); }); + return std::ranges::any_of(vec, [](double x) -> bool { return std::isnan(x); }); } auto all_nan(const std::vector & vec) -> bool { - return std::ranges::all_of(vec, [](double x) { return std::isnan(x); }); + return std::ranges::all_of(vec, [](double x) -> bool { return std::isnan(x); }); } auto isclose(double a, double b, double rtol, double atol) -> bool { - return std::abs(a - b) <= (atol + rtol * std::abs(b)); + return std::abs(a - b) <= (atol + (rtol * std::abs(b))); } } // namespace math diff --git a/controller_coordinator/CHANGELOG.md b/controller_coordinator/CHANGELOG.md index 1dd2368..1464936 100644 --- a/controller_coordinator/CHANGELOG.md +++ b/controller_coordinator/CHANGELOG.md @@ -1,5 +1,7 @@ # Changelog for package controller_coordinator +## 0.5.1 (2026-08-14) + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/controller_coordinator/package.xml b/controller_coordinator/package.xml index 8be4470..6877179 100644 --- a/controller_coordinator/package.xml +++ b/controller_coordinator/package.xml @@ -3,7 +3,7 @@ controller_coordinator - 0.5.0 + 0.5.1 A high-level node used to load and activate/deactivate control systems Evan Palmer diff --git a/controller_coordinator/src/coordinator.cpp b/controller_coordinator/src/coordinator.cpp index 5b61084..a5a4015 100644 --- a/controller_coordinator/src/coordinator.cpp +++ b/controller_coordinator/src/coordinator.cpp @@ -21,9 +21,10 @@ #include "coordinator.hpp" #include -#include #include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" namespace coordinator { diff --git a/controller_coordinator/src/coordinator.hpp b/controller_coordinator/src/coordinator.hpp index e07ee92..ec9d323 100644 --- a/controller_coordinator/src/coordinator.hpp +++ b/controller_coordinator/src/coordinator.hpp @@ -20,13 +20,8 @@ #pragma once -#include "controller_manager_msgs/srv/configure_controller.hpp" -#include "controller_manager_msgs/srv/list_controllers.hpp" -#include "controller_manager_msgs/srv/load_controller.hpp" #include "controller_manager_msgs/srv/set_hardware_component_state.hpp" #include "controller_manager_msgs/srv/switch_controller.hpp" -#include "controller_manager_msgs/srv/unload_controller.hpp" -#include "rclcpp/rclcpp.hpp" #include "std_srvs/srv/set_bool.hpp" // auto-generated by generate_parameter_library diff --git a/ik_solvers/CHANGELOG.md b/ik_solvers/CHANGELOG.md index 81967be..2ae5204 100644 --- a/ik_solvers/CHANGELOG.md +++ b/ik_solvers/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package ik_solvers +## 0.5.1 (2026-08-14) + +- Declares the `python3-numpy` build dependency required by Pinocchio's Python bindings + ## 0.5.0 (2026-05-29) - Replace the quaternion error with the left-invariant error diff --git a/ik_solvers/include/ik_solvers/solver.hpp b/ik_solvers/include/ik_solvers/solver.hpp index d0afe1b..38a482e 100644 --- a/ik_solvers/include/ik_solvers/solver.hpp +++ b/ik_solvers/include/ik_solvers/solver.hpp @@ -24,7 +24,7 @@ #include #include -#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/multibody/fwd.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" diff --git a/ik_solvers/include/ik_solvers/task_priority_solver.hpp b/ik_solvers/include/ik_solvers/task_priority_solver.hpp index 866dd76..a7345c1 100644 --- a/ik_solvers/include/ik_solvers/task_priority_solver.hpp +++ b/ik_solvers/include/ik_solvers/task_priority_solver.hpp @@ -20,14 +20,12 @@ #pragma once -#include #include #include #include #include #include "ik_solvers/solver.hpp" -#include "rclcpp/rclcpp.hpp" // auto-generated by generate_parameter_library #include diff --git a/ik_solvers/package.xml b/ik_solvers/package.xml index a8698c2..7de6a5e 100644 --- a/ik_solvers/package.xml +++ b/ik_solvers/package.xml @@ -3,7 +3,7 @@ ik_solvers - 0.5.0 + 0.5.1 Inverse kinematics solvers used for whole-body control Evan Palmer @@ -15,6 +15,7 @@ Evan Palmer ament_cmake + ament_cmake_ros eigen3_cmake_module eigen @@ -24,6 +25,9 @@ trajectory_msgs generate_parameter_library pinocchio + hydrodynamics + python3-numpy + tf2_eigen ament_cmake diff --git a/ik_solvers/src/pseudoinverse.cpp b/ik_solvers/src/pseudoinverse.cpp index 2e5532e..ba6b063 100644 --- a/ik_solvers/src/pseudoinverse.cpp +++ b/ik_solvers/src/pseudoinverse.cpp @@ -1,7 +1,5 @@ #include "pseudoinverse.hpp" -#include - namespace ik_solvers::pinv { diff --git a/ik_solvers/src/pseudoinverse.hpp b/ik_solvers/src/pseudoinverse.hpp index 7717ede..a0866e8 100644 --- a/ik_solvers/src/pseudoinverse.hpp +++ b/ik_solvers/src/pseudoinverse.hpp @@ -20,8 +20,6 @@ #include -#include "rclcpp/rclcpp.hpp" - namespace ik_solvers::pinv { diff --git a/ik_solvers/src/solver.cpp b/ik_solvers/src/solver.cpp index 7aa8586..241a5ed 100644 --- a/ik_solvers/src/solver.cpp +++ b/ik_solvers/src/solver.cpp @@ -54,7 +54,7 @@ auto IKSolver::solve(const rclcpp::Duration & period, const Eigen::Isometry3d & if (!result.has_value()) { return std::unexpected(result.error()); } - const Eigen::VectorXd solution = result.value(); + const Eigen::VectorXd & solution = result.value(); // Integrate the solution to get the new joint positions const Eigen::VectorXd q_next = pinocchio::integrate(*model_, q, period.seconds() * solution); diff --git a/ik_solvers/src/task_priority_solver.cpp b/ik_solvers/src/task_priority_solver.cpp index 17a4fa2..8fb91a5 100644 --- a/ik_solvers/src/task_priority_solver.cpp +++ b/ik_solvers/src/task_priority_solver.cpp @@ -28,10 +28,7 @@ #include "hydrodynamics/hydrodynamics.hpp" #include "pinocchio/algorithm/frames.hpp" -#include "pinocchio/algorithm/jacobian.hpp" -#include "pinocchio/algorithm/kinematics.hpp" #include "pseudoinverse.hpp" -#include "tf2_eigen/tf2_eigen.hpp" namespace ik_solvers { @@ -47,7 +44,7 @@ auto active_tasks(const ConstraintSet & tasks) -> ConstraintSet { ConstraintSet result; std::ranges::copy( - tasks | std::views::filter([](const auto & task) { + tasks | std::views::filter([](const auto & task) -> auto { auto set_task = std::dynamic_pointer_cast(task); return !std::dynamic_pointer_cast(set_task) || set_task->is_active(); }), @@ -79,8 +76,8 @@ auto TaskHierarchy::set_constraints() const -> ConstraintSet // NOLINT const ConstraintSet tasks = active_tasks(constraints_); ConstraintSet result; std::ranges::copy( - tasks | - std::views::filter([](const auto & task) { return std::dynamic_pointer_cast(task) != nullptr; }), + tasks | std::views::filter( + [](const auto & task) -> auto { return std::dynamic_pointer_cast(task) != nullptr; }), std::inserter(result, result.end())); return result; } @@ -90,7 +87,8 @@ auto TaskHierarchy::equality_constraints() const -> ConstraintSet // NOLINT const ConstraintSet tasks = active_tasks(constraints_); ConstraintSet result; std::ranges::copy( - tasks | std::views::filter([](const auto & task) { return !std::dynamic_pointer_cast(task); }), + tasks | + std::views::filter([](const auto & task) -> auto { return !std::dynamic_pointer_cast(task); }), std::inserter(result, result.end())); return result; } @@ -180,8 +178,10 @@ auto construct_augmented_jacobian(const std::vector & jacobians } const int n_cols = jacobians.front().cols(); - const int n_rows = std::accumulate( - jacobians.begin(), jacobians.end(), 0, [](int sum, const Eigen::MatrixXd & jac) { return sum + jac.rows(); }); + const int n_rows = + std::accumulate(jacobians.begin(), jacobians.end(), 0, [](int sum, const Eigen::MatrixXd & jac) -> int { + return sum + jac.rows(); + }); Eigen::MatrixXd augmented_jacobian(n_rows, n_cols); int current_row = 0; // NOLINT(misc-const-correctness) @@ -222,7 +222,7 @@ auto tpik(const hierarchy::ConstraintSet & tasks, size_t nv, double damping) /// Check if the solution is feasible. auto is_feasible(const hierarchy::ConstraintSet & constraints, const Eigen::VectorXd & solution) -> bool { - return std::ranges::all_of(constraints, [&solution](const auto & constraint) { + return std::ranges::all_of(constraints, [&solution](const auto & constraint) -> auto { auto set_task = std::dynamic_pointer_cast(constraint); const double pred = (constraint->jacobian() * solution).value(); return ( @@ -267,7 +267,7 @@ auto search_solutions( } // Choose the solution with the smallest norm - return *std::ranges::min_element(solutions, {}, [](const auto & a) { return a.norm(); }); + return *std::ranges::min_element(solutions, {}, [](const auto & a) -> auto { return a.norm(); }); } auto pinocchio_to_eigen(const pinocchio::SE3 & pose) -> Eigen::Isometry3d diff --git a/impedance_controller/CHANGELOG.md b/impedance_controller/CHANGELOG.md index 9afa89b..910f64d 100644 --- a/impedance_controller/CHANGELOG.md +++ b/impedance_controller/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package impedance_controller +## 0.5.1 (2026-08-14) + +- Declares dependencies that were previously pulled in transitively + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/impedance_controller/include/impedance_controller/impedance_controller.hpp b/impedance_controller/include/impedance_controller/impedance_controller.hpp index 8378fff..6590e03 100644 --- a/impedance_controller/include/impedance_controller/impedance_controller.hpp +++ b/impedance_controller/include/impedance_controller/impedance_controller.hpp @@ -25,17 +25,11 @@ #include "auv_control_msgs/msg/impedance_command.hpp" #include "auv_control_msgs/msg/impedance_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "controller_interface/controller_interface.hpp" #include "hydrodynamics/hydrodynamics.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" -#include "std_msgs/msg/string.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" // auto-generated by generate_parameter_library #include diff --git a/impedance_controller/package.xml b/impedance_controller/package.xml index a8a1f4b..7df6476 100644 --- a/impedance_controller/package.xml +++ b/impedance_controller/package.xml @@ -3,7 +3,7 @@ impedance_controller - 0.5.0 + 0.5.1 An impedance controller for underwater vehicles Evan Palmer @@ -26,11 +26,15 @@ rclcpp_lifecycle generate_parameter_library tf2_eigen + tf2 + tf2_ros geometry_msgs nav_msgs controller_common hydrodynamics auv_control_msgs + control_msgs + realtime_tools ament_lint_auto ament_lint_common diff --git a/impedance_controller/src/impedance_controller.cpp b/impedance_controller/src/impedance_controller.cpp index fa8d31b..090842e 100644 --- a/impedance_controller/src/impedance_controller.cpp +++ b/impedance_controller/src/impedance_controller.cpp @@ -78,9 +78,10 @@ auto ImpedanceController::configure_parameters() -> controller_interface::Callba n_state_dofs_ = state_dofs_.size(); n_reference_dofs_ = n_command_dofs_ + n_state_dofs_; - auto get_gains = [this](auto field) { - auto gains = command_dofs_ | - std::views::transform([&](const auto & dof) { return params_.gains.command_joints_map[dof].*field; }); + auto get_gains = [this](auto field) -> auto { + auto gains = command_dofs_ | std::views::transform([&](const auto & dof) -> auto { + return params_.gains.command_joints_map[dof].*field; + }); return std::vector(gains.begin(), gains.end()); }; @@ -236,7 +237,7 @@ auto ImpedanceController::update_system_state_values() -> controller_interface:: auto * current_state = system_state_.readFromRT(); std::ranges::copy(common::messages::to_vector(*current_state), system_state_values_.begin()); } else { - std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) { + std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) -> auto { return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); }); } @@ -311,7 +312,7 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c } // convert the reference wrench values into an Eigen vector - Eigen::Vector6d reference_wrench(ref_wrench_values.data()); + const Eigen::Vector6d reference_wrench(ref_wrench_values.data()); // calculate the control command Eigen::Vector6d t = reference_wrench + kp_ * pose_error + kd_ * twist_error; @@ -333,7 +334,7 @@ auto ImpedanceController::update_and_write_commands(const rclcpp::Time & time, c common::messages::to_msg(twist_error_values, &controller_state_.error_twist); controller_state_.time_step = period.seconds(); - std::vector output_values(t.data(), t.data() + t.size()); + const std::vector output_values(t.data(), t.data() + t.size()); common::messages::to_msg(output_values, &controller_state_.output); rt_controller_state_pub_->try_publish(controller_state_); diff --git a/ros2.repos b/ros2.repos index 87dff95..7f23e58 100644 --- a/ros2.repos +++ b/ros2.repos @@ -4,8 +4,3 @@ repositories: type: git url: https://github.com/Robotic-Decision-Making-Lab/hydrodynamics.git version: main - - mobile_to_maritime: - type: git - url: https://github.com/Robotic-Decision-Making-Lab/mobile_to_maritime.git - version: main diff --git a/thruster_allocation_matrix_controller/CHANGELOG.md b/thruster_allocation_matrix_controller/CHANGELOG.md index 090db7c..9649131 100644 --- a/thruster_allocation_matrix_controller/CHANGELOG.md +++ b/thruster_allocation_matrix_controller/CHANGELOG.md @@ -1,5 +1,10 @@ # Changelog for package thruster_allocation_matrix_controller +## 0.5.1 (2026-08-14) + +- Fixes a format-string bug in the controller's error logging +- Declares dependencies that were previously pulled in transitively + ## 0.5.0 (2026-05-29) - Set NaN reference force/torque to zero thrust. 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 a8c0aa8..0dd56fd 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 @@ -21,7 +21,6 @@ #pragma once #include -#include #include #include #include @@ -29,11 +28,7 @@ #include "auv_control_msgs/msg/multi_actuator_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/wrench.hpp" -#include "hydrodynamics/hydrodynamics.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" diff --git a/thruster_allocation_matrix_controller/package.xml b/thruster_allocation_matrix_controller/package.xml index 2ee30c6..e7a3ed8 100644 --- a/thruster_allocation_matrix_controller/package.xml +++ b/thruster_allocation_matrix_controller/package.xml @@ -3,7 +3,7 @@ thruster_allocation_matrix_controller - 0.5.0 + 0.5.1 Thruster allocation matrix controller used to convert wrench commands into thrust commands Evan Palmer @@ -31,6 +31,8 @@ auv_control_msgs hydrodynamics controller_common + geometry_msgs + realtime_tools eigen3_cmake_module 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 4cfa171..ac9064f 100644 --- a/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp +++ b/thruster_allocation_matrix_controller/src/thruster_allocation_matrix_controller.cpp @@ -24,10 +24,10 @@ #include #include #include -#include #include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hydrodynamics/hydrodynamics.hpp" namespace thruster_allocation_matrix_controller { @@ -62,6 +62,7 @@ auto ThrusterAllocationMatrixController::configure_parameters() -> controller_in if (!params_.reference_controllers.empty() && params_.reference_controllers.size() != n_thrusters_) { RCLCPP_ERROR( get_node()->get_logger(), + "%s", std::format( "Mismatched number of command interface prefixes and thrusters. Expected {}, got {}.", n_thrusters_, @@ -75,9 +76,10 @@ auto ThrusterAllocationMatrixController::configure_parameters() -> controller_in 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 - if (std::ranges::any_of(vecs, [this](const auto & vec) { return vec.size() != n_thrusters_; })) { + if (std::ranges::any_of(vecs, [this](const auto & vec) -> auto { return vec.size() != n_thrusters_; })) { RCLCPP_ERROR( get_node()->get_logger(), + "%s", std::format("Mismatched TAM row sizes. Expected {}, got {}.", n_thrusters_, vecs[0].size()).c_str()); return controller_interface::CallbackReturn::ERROR; @@ -202,7 +204,9 @@ auto ThrusterAllocationMatrixController::update_and_write_commands( for (auto && [interface, value] : std::views::zip(command_interfaces_, thrust)) { if (!interface.set_value(value)) { RCLCPP_INFO( - get_node()->get_logger(), std::format("Failed to set command for thruster {}", interface.get_name()).c_str()); + get_node()->get_logger(), + "%s", + std::format("Failed to set command for thruster {}", interface.get_name()).c_str()); return controller_interface::return_type::ERROR; } } diff --git a/thruster_controllers/CHANGELOG.md b/thruster_controllers/CHANGELOG.md index cce960f..b75fbd3 100644 --- a/thruster_controllers/CHANGELOG.md +++ b/thruster_controllers/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package thruster_controllers +## 0.5.1 (2026-08-14) + +- Declares dependencies that were previously pulled in transitively + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp b/thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp index bc81140..cd02c9c 100644 --- a/thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp +++ b/thruster_controllers/include/thruster_controllers/gz_passthrough_controller.hpp @@ -26,9 +26,6 @@ #include "control_msgs/msg/single_dof_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "controller_interface/controller_interface.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" 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 35f879b..811b0e4 100644 --- a/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp +++ b/thruster_controllers/include/thruster_controllers/polynomial_thrust_curve_controller.hpp @@ -26,9 +26,6 @@ #include "control_msgs/msg/single_dof_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "controller_interface/controller_interface.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" diff --git a/thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp b/thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp index d082e89..7ff95dd 100644 --- a/thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp +++ b/thruster_controllers/include/thruster_controllers/rotation_rate_controller.hpp @@ -26,9 +26,6 @@ #include "control_msgs/msg/single_dof_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "controller_interface/controller_interface.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" diff --git a/thruster_controllers/package.xml b/thruster_controllers/package.xml index d51d3d8..59bb8a6 100644 --- a/thruster_controllers/package.xml +++ b/thruster_controllers/package.xml @@ -3,7 +3,7 @@ thruster_controllers - 0.5.0 + 0.5.1 A collection of thruster controllers for AUV control Evan Palmer @@ -25,6 +25,8 @@ generate_parameter_library control_msgs controller_common + realtime_tools + std_msgs ament_cmake diff --git a/thruster_controllers/src/gz_passthrough_controller.cpp b/thruster_controllers/src/gz_passthrough_controller.cpp index e9a4292..fd2a0ac 100644 --- a/thruster_controllers/src/gz_passthrough_controller.cpp +++ b/thruster_controllers/src/gz_passthrough_controller.cpp @@ -20,11 +20,6 @@ #include "thruster_controllers/gz_passthrough_controller.hpp" -#include -#include -#include -#include - #include "hardware_interface/types/hardware_interface_type_values.hpp" namespace thruster_controllers @@ -131,7 +126,7 @@ auto GazeboPassthroughController::update_and_write_commands(const rclcpp::Time & const double reference = reference_interfaces_[0]; std_msgs::msg::Float64 msg = std_msgs::msg::Float64(); msg.data = reference; - passthrough_pub_->publish(msg); + passthrough_pub_->publish(msg); // NOLINT(portability-template-virtual-member-function) controller_state_.header.stamp = time; controller_state_.dof_state.reference = reference_interfaces_[0]; diff --git a/thruster_controllers/src/polynomial_thrust_curve_controller.cpp b/thruster_controllers/src/polynomial_thrust_curve_controller.cpp index 41c5ec6..6f6b1e9 100644 --- a/thruster_controllers/src/polynomial_thrust_curve_controller.cpp +++ b/thruster_controllers/src/polynomial_thrust_curve_controller.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/thruster_controllers/src/rotation_rate_controller.cpp b/thruster_controllers/src/rotation_rate_controller.cpp index d6207fa..140273d 100644 --- a/thruster_controllers/src/rotation_rate_controller.cpp +++ b/thruster_controllers/src/rotation_rate_controller.cpp @@ -22,8 +22,6 @@ #include #include -#include -#include #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/topic_sensors/CHANGELOG.md b/topic_sensors/CHANGELOG.md index db6554d..2ac0aa9 100644 --- a/topic_sensors/CHANGELOG.md +++ b/topic_sensors/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package topic_sensors +## 0.5.1 (2026-08-14) + +- Removes the mobile-to-maritime message transform option from the odom sensor + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/topic_sensors/CMakeLists.txt b/topic_sensors/CMakeLists.txt index 5027d18..721ed75 100644 --- a/topic_sensors/CMakeLists.txt +++ b/topic_sensors/CMakeLists.txt @@ -13,7 +13,6 @@ find_package(realtime_tools REQUIRED) find_package(std_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(controller_common REQUIRED) -find_package(message_transforms REQUIRED) find_package(ament_cmake REQUIRED) find_package(pluginlib REQUIRED) @@ -35,7 +34,6 @@ target_link_libraries( rclcpp::rclcpp realtime_tools::realtime_tools controller_common::controller_common - message_transforms::message_transforms ${std_msgs_TARGETS} ${nav_msgs_TARGETS} ) @@ -59,7 +57,6 @@ ament_export_dependencies( "std_msgs" "nav_msgs" "controller_common" - "message_transforms" ) ament_package() diff --git a/topic_sensors/README.md b/topic_sensors/README.md index 38e50bc..de84c67 100644 --- a/topic_sensors/README.md +++ b/topic_sensors/README.md @@ -16,6 +16,3 @@ topic_sensors/odom_sensor - prefix: The node prefix. - topic: The topic that the sensor should subscribe to. -- transform_message: Whether or not the sensor should transform the message - from the ROS [REP-105](https://ros.org/reps/rep-0105.html) convention to the - maritime convention documented in [REP-156](https://github.com/ros-infrastructure/rep/pull/398). diff --git a/topic_sensors/include/topic_sensors/odom_sensor.hpp b/topic_sensors/include/topic_sensors/odom_sensor.hpp index 0821082..b05b15a 100644 --- a/topic_sensors/include/topic_sensors/odom_sensor.hpp +++ b/topic_sensors/include/topic_sensors/odom_sensor.hpp @@ -26,11 +26,8 @@ #include #include "hardware_interface/sensor_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/executors/single_threaded_executor.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "realtime_tools/realtime_buffer.hpp" namespace topic_sensors diff --git a/topic_sensors/package.xml b/topic_sensors/package.xml index 50ad85f..ba417d9 100644 --- a/topic_sensors/package.xml +++ b/topic_sensors/package.xml @@ -3,7 +3,7 @@ topic_sensors - 0.5.0 + 0.5.1 Sensor plugins used to write ROS 2 messages to state interfaces Evan Palmer @@ -25,7 +25,8 @@ geometry_msgs generate_parameter_library controller_common - message_transforms + realtime_tools + std_msgs ament_cmake diff --git a/topic_sensors/src/odom_sensor.cpp b/topic_sensors/src/odom_sensor.cpp index 66b10de..b610aa8 100644 --- a/topic_sensors/src/odom_sensor.cpp +++ b/topic_sensors/src/odom_sensor.cpp @@ -24,7 +24,6 @@ #include #include "controller_common/common.hpp" -#include "message_transforms/transforms.hpp" namespace topic_sensors { @@ -52,19 +51,10 @@ auto OdomSensor::on_configure(const rclcpp_lifecycle::State & /*previous_state*/ } RCLCPP_INFO(logger_, "Subscribing to topic: %s", topic.c_str()); // NOLINT - const bool transform_message = info_.hardware_parameters.at("transform_message") == "true"; - if (transform_message) { - // NOLINTNEXTLINE - RCLCPP_INFO(logger_, "Incoming messages will be transform from the ROS mobile standard to the maritime standard"); - } - state_sub_ = node_->create_subscription( topic, rclcpp::SensorDataQoS(), - [this, &transform_message](const std::shared_ptr msg) { // NOLINT - if (transform_message) { - m2m::transform_message(*msg, "map_ned", "base_link_fsd"); - } + [this](const std::shared_ptr msg) { // NOLINT state_.writeFromNonRT(*msg); }); return hardware_interface::CallbackReturn::SUCCESS; diff --git a/trajectory_controllers/CHANGELOG.md b/trajectory_controllers/CHANGELOG.md index 5b68440..b2baed0 100644 --- a/trajectory_controllers/CHANGELOG.md +++ b/trajectory_controllers/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package trajectory_controllers +## 0.5.1 (2026-08-14) + +- Declares dependencies that were previously pulled in transitively + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp index 8d874a5..3b6aabe 100644 --- a/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp +++ b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory.hpp @@ -21,13 +21,12 @@ #pragma once #include -#include #include #include #include "auv_control_msgs/msg/cartesian_trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" namespace trajectory_controllers { diff --git a/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp index c78dfa9..4892f2e 100644 --- a/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp +++ b/trajectory_controllers/include/trajectory_controllers/cartesian_trajectory_controller.hpp @@ -26,12 +26,10 @@ #include "auv_control_msgs/msg/cartesian_trajectory_controller_state_stamped.hpp" #include "controller_common/common.hpp" #include "controller_interface/controller_interface.hpp" -#include "rclcpp/rclcpp.hpp" #include "rclcpp_action/server.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" #include "realtime_tools/realtime_server_goal_handle.hpp" -#include "tf2/exceptions.hpp" #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" #include "trajectory_controllers/cartesian_trajectory.hpp" diff --git a/trajectory_controllers/package.xml b/trajectory_controllers/package.xml index db8ab66..33c84ba 100644 --- a/trajectory_controllers/package.xml +++ b/trajectory_controllers/package.xml @@ -3,7 +3,7 @@ trajectory_controllers - 0.5.0 + 0.5.1 Trajectory controllers for underwater vehicles and manipulator systems Evan Palmer @@ -30,6 +30,12 @@ auv_control_msgs rclcpp_action lifecycle_msgs + pluginlib + realtime_tools + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros ament_cmake diff --git a/trajectory_controllers/src/cartesian_trajectory.cpp b/trajectory_controllers/src/cartesian_trajectory.cpp index 552f5c4..89dab8b 100644 --- a/trajectory_controllers/src/cartesian_trajectory.cpp +++ b/trajectory_controllers/src/cartesian_trajectory.cpp @@ -25,7 +25,6 @@ #include "controller_common/common.hpp" #include "tf2_eigen/tf2_eigen.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace trajectory_controllers { @@ -145,7 +144,11 @@ auto CartesianTrajectory::sample(const rclcpp::Time & sample_time) const // the sample time is before the first point in the trajectory, so we need to interpolate between the starting // state and the first point in the trajectory if (sample_time < start_time()) { - return interpolate(initial_state_, start_point().value(), initial_time_, start_time(), sample_time); + const auto point = start_point(); + if (!point.has_value()) { + return std::unexpected(SampleError::EMPTY_TRAJECTORY); + } + return interpolate(initial_state_, point.value(), initial_time_, start_time(), sample_time); } for (const auto [p1, p2] : std::views::zip(points_.points, points_.points | std::views::drop(1))) { diff --git a/trajectory_controllers/src/cartesian_trajectory_controller.cpp b/trajectory_controllers/src/cartesian_trajectory_controller.cpp index 5ab13bc..4e7437e 100644 --- a/trajectory_controllers/src/cartesian_trajectory_controller.cpp +++ b/trajectory_controllers/src/cartesian_trajectory_controller.cpp @@ -21,12 +21,12 @@ #include "trajectory_controllers/cartesian_trajectory_controller.hpp" #include +#include #include #include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp_action/rclcpp_action.hpp" #include "tf2_eigen/tf2_eigen.hpp" namespace trajectory_controllers @@ -173,8 +173,8 @@ auto CartesianTrajectoryController::on_configure(const rclcpp_lifecycle::State & auto handle_goal = [this]( const rclcpp_action::GoalUUID & /*uuid*/, - std::shared_ptr goal) { // NOLINT - RCLCPP_INFO(logger_, "Received new trajectory goal"); // NOLINT + std::shared_ptr goal) -> rclcpp_action::GoalResponse { // NOLINT + RCLCPP_INFO(logger_, "Received new trajectory goal"); // NOLINT if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR(logger_, "Can't accept new action goals. Controller is not running."); // NOLINT return rclcpp_action::GoalResponse::REJECT; @@ -229,8 +229,9 @@ auto CartesianTrajectoryController::on_configure(const rclcpp_lifecycle::State & rt_gh->execute(); rt_active_goal_.writeFromNonRT(rt_gh); - goal_handle_timer_.reset(); - goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_, [rt_gh]() { rt_gh->runNonRealtime(); }); + goal_handle_timer_ = nullptr; + goal_handle_timer_ = + get_node()->create_wall_timer(action_monitor_period_, [rt_gh]() -> void { rt_gh->runNonRealtime(); }); }; action_server_ = rclcpp_action::create_server( @@ -259,7 +260,7 @@ auto CartesianTrajectoryController::command_interface_configuration() const config.type = controller_interface::interface_configuration_type::INDIVIDUAL; config.names.reserve(n_dofs_); - std::ranges::transform(dofs_, std::back_inserter(config.names), [this](const auto & dof) { + std::ranges::transform(dofs_, std::back_inserter(config.names), [this](const auto & dof) -> auto { return params_.reference_controller.empty() ? std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION) : std::format("{}/{}/{}", params_.reference_controller, dof, hardware_interface::HW_IF_POSITION); @@ -279,7 +280,7 @@ auto CartesianTrajectoryController::state_interface_configuration() const config.type = controller_interface::interface_configuration_type::INDIVIDUAL; config.names.reserve(n_dofs_); - std::ranges::transform(dofs_, std::back_inserter(config.names), [](const auto & dof) { + std::ranges::transform(dofs_, std::back_inserter(config.names), [](const auto & dof) -> auto { return std::format("{}/{}", dof, hardware_interface::HW_IF_POSITION); }); @@ -345,7 +346,7 @@ auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclc auto command_state = system_state; double error = std::numeric_limits::quiet_NaN(); - auto publish_controller_state = [this, &reference_state, &system_state, &error, &command_state]() { + auto publish_controller_state = [this, &reference_state, &system_state, &error, &command_state]() -> void { controller_state_.header.stamp = get_node()->now(); controller_state_.reference = reference_state; controller_state_.feedback = system_state; @@ -406,8 +407,13 @@ auto CartesianTrajectoryController::update(const rclcpp::Time & time, const rclc break; case SampleError::SAMPLE_TIME_AFTER_END: { + const auto end_point = trajectory->end_point(); + if (!end_point.has_value()) { + break; + } + const double goal_tolerance = *rt_goal_tolerance_.readFromRT(); - const double goal_error = geodesic_error(trajectory->end_point().value(), system_state); + const double goal_error = geodesic_error(end_point.value(), system_state); RCLCPP_INFO(logger_, "CartesianTrajectory sample time is after trajectory end time."); // NOLINT if (goal_tolerance > 0.0) { if (goal_error > goal_tolerance) { diff --git a/twist_controllers/CHANGELOG.md b/twist_controllers/CHANGELOG.md index e6ddafc..c6d8a3d 100644 --- a/twist_controllers/CHANGELOG.md +++ b/twist_controllers/CHANGELOG.md @@ -1,5 +1,9 @@ # Changelog for package twist_controllers +## 0.5.1 (2026-08-14) + +- Declares dependencies that were previously pulled in transitively + ## 0.5.0 (2026-05-29) - Rename `velocity_controllers` to `twist_controllers` to avoid name collision diff --git a/twist_controllers/include/twist_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp b/twist_controllers/include/twist_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp index 861c51d..d0f37ac 100644 --- a/twist_controllers/include/twist_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp +++ b/twist_controllers/include/twist_controllers/adaptive_integral_terminal_sliding_mode_controller.hpp @@ -24,16 +24,11 @@ #include "control_msgs/msg/multi_dof_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "controller_interface/controller_interface.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" #include "hydrodynamics/hydrodynamics.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" -#include "std_msgs/msg/string.hpp" #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" diff --git a/twist_controllers/include/twist_controllers/integral_sliding_mode_controller.hpp b/twist_controllers/include/twist_controllers/integral_sliding_mode_controller.hpp index 2fcfd48..2350215 100644 --- a/twist_controllers/include/twist_controllers/integral_sliding_mode_controller.hpp +++ b/twist_controllers/include/twist_controllers/integral_sliding_mode_controller.hpp @@ -21,7 +21,6 @@ #pragma once #include -#include #include #include #include @@ -29,16 +28,11 @@ #include "control_msgs/msg/multi_dof_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "controller_interface/controller_interface.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" #include "hydrodynamics/hydrodynamics.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" -#include "std_msgs/msg/string.hpp" #include "tf2_ros/buffer.hpp" #include "tf2_ros/transform_listener.hpp" diff --git a/twist_controllers/package.xml b/twist_controllers/package.xml index 07c4f40..88117b8 100644 --- a/twist_controllers/package.xml +++ b/twist_controllers/package.xml @@ -3,7 +3,7 @@ twist_controllers - 0.5.0 + 0.5.1 A collection of velocity controllers for underwater vehicles Evan Palmer @@ -31,6 +31,9 @@ geometry_msgs nav_msgs controller_common + realtime_tools + tf2 + tf2_ros ament_lint_auto ament_lint_common diff --git a/twist_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp b/twist_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp index 3f1054a..ab7d04d 100644 --- a/twist_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp +++ b/twist_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp @@ -39,7 +39,7 @@ auto sign(double x, double thickness) -> double { return std::tanh(x / thickness /// Apply the element-wise sign function to a vector using tanh and a given boundary thickness. auto sign(const Eigen::Vector6d & x, double thickness) -> Eigen::Vector6d { - return x.unaryExpr([thickness](double val) { return sign(val, thickness); }); + return x.unaryExpr([thickness](double val) -> double { return sign(val, thickness); }); } } // namespace @@ -80,8 +80,9 @@ auto AdaptiveIntegralTerminalSlidingModeController::configure_parameters() -> co n_dofs_ = dofs_.size(); lambda_ = params_.gains.lambda; - auto get_gain = [this](auto field) { - auto gain = dofs_ | std::views::transform([&](const auto & dof) { return params_.gains.joints_map[dof].*field; }); + auto get_gain = [this](auto field) -> auto { + auto gain = + dofs_ | std::views::transform([&](const auto & dof) -> auto { return params_.gains.joints_map[dof].*field; }); return std::vector(gain.begin(), gain.end()); }; @@ -237,7 +238,7 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_system_state_values() std::ranges::copy(common::messages::to_vector(current_state->twist.twist), system_state_values_.begin()); tf2::fromMsg(current_state->pose.pose.orientation, *system_rotation_.readFromRT()); } else { - std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) { + std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) -> auto { return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); }); diff --git a/twist_controllers/src/integral_sliding_mode_controller.cpp b/twist_controllers/src/integral_sliding_mode_controller.cpp index bdd9591..0bcd988 100644 --- a/twist_controllers/src/integral_sliding_mode_controller.cpp +++ b/twist_controllers/src/integral_sliding_mode_controller.cpp @@ -21,11 +21,7 @@ #include "twist_controllers/integral_sliding_mode_controller.hpp" #include -#include #include -#include -#include -#include #include #include #include @@ -81,8 +77,9 @@ auto IntegralSlidingModeController::configure_parameters() -> controller_interfa n_dofs_ = dofs_.size(); boundary_thickness_ = params_.gains.lambda; - auto get_gain = [this](auto field) { - auto gain = dofs_ | std::views::transform([&](const auto & dof) { return params_.gains.joints_map[dof].*field; }); + auto get_gain = [this](auto field) -> auto { + auto gain = + dofs_ | std::views::transform([&](const auto & dof) -> auto { return params_.gains.joints_map[dof].*field; }); return std::vector(gain.begin(), gain.end()); }; @@ -230,7 +227,7 @@ auto IntegralSlidingModeController::update_system_state_values() -> controller_i std::ranges::copy(common::messages::to_vector(current_state->twist.twist), system_state_values_.begin()); tf2::fromMsg(current_state->pose.pose.orientation, *system_rotation_.readFromRT()); } else { - std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) { + std::ranges::transform(state_interfaces_, system_state_values_.begin(), [](const auto & interface) -> auto { return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); }); @@ -303,7 +300,7 @@ auto IntegralSlidingModeController::update_and_write_commands( // calculate the disturbance rejection torque Eigen::Vector6d surface = error + kp_ * total_error_ - kp_ * init_error_; - surface = surface.unaryExpr([this](double x) { return std::tanh(x / boundary_thickness_); }); + surface = surface.unaryExpr([this](double x) -> double { return std::tanh(x / boundary_thickness_); }); const Eigen::Vector6d tau1 = rho_ * surface; // total control torque diff --git a/whole_body_controllers/CHANGELOG.md b/whole_body_controllers/CHANGELOG.md index 12f158f..4f63de6 100644 --- a/whole_body_controllers/CHANGELOG.md +++ b/whole_body_controllers/CHANGELOG.md @@ -1,5 +1,10 @@ # Changelog for package whole_body_controllers +## 0.5.1 (2026-08-14) + +- Removes the mobile-to-maritime message transform from the IK controller +- Declares dependencies that were previously pulled in transitively + ## 0.5.0 (2026-05-29) ## 0.4.3 (2026-04-25) diff --git a/whole_body_controllers/CMakeLists.txt b/whole_body_controllers/CMakeLists.txt index 496c482..064344a 100644 --- a/whole_body_controllers/CMakeLists.txt +++ b/whole_body_controllers/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(pinocchio REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) -find_package(message_transforms REQUIRED) find_package(controller_common REQUIRED) find_package(ik_solvers REQUIRED) find_package(auv_control_msgs REQUIRED) @@ -55,7 +54,6 @@ target_link_libraries( tf2_ros::tf2_ros ik_solvers::ik_solvers tf2_eigen::tf2_eigen - message_transforms::message_transforms controller_common::controller_common Eigen3::Eigen ${geometry_msgs_TARGETS} @@ -89,7 +87,6 @@ ament_export_dependencies( "geometry_msgs" "tf2_ros" "tf2_eigen" - "message_transforms" "controller_common" "ik_solvers" "auv_control_msgs" diff --git a/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp index 19db28d..eb7e9e2 100644 --- a/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp +++ b/whole_body_controllers/include/whole_body_controllers/ik_controller.hpp @@ -22,20 +22,12 @@ #include "auv_control_msgs/msg/ik_controller_state_stamped.hpp" #include "controller_interface/chainable_controller_interface.hpp" -#include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" #include "ik_solvers/solver.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/kinematics.hpp" #include "pluginlib/class_loader.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "realtime_tools/realtime_buffer.hpp" #include "realtime_tools/realtime_publisher.hpp" -#include "std_msgs/msg/string.hpp" -#include "tf2_ros/buffer.hpp" -#include "tf2_ros/transform_listener.hpp" // auto-generated by generate_parameter_library #include diff --git a/whole_body_controllers/package.xml b/whole_body_controllers/package.xml index 888ba2b..eeb2fe6 100644 --- a/whole_body_controllers/package.xml +++ b/whole_body_controllers/package.xml @@ -3,7 +3,7 @@ whole_body_controllers - 0.5.0 + 0.5.1 Whole-body controllers for underwater vehicle manipulator systems Evan Palmer @@ -26,10 +26,14 @@ control_msgs pinocchio nav_msgs - message_transforms controller_common ik_solvers auv_control_msgs + geometry_msgs + realtime_tools + std_msgs + tf2_eigen + tf2_ros ament_cmake diff --git a/whole_body_controllers/src/ik_controller.cpp b/whole_body_controllers/src/ik_controller.cpp index 22b6bc1..229272a 100644 --- a/whole_body_controllers/src/ik_controller.cpp +++ b/whole_body_controllers/src/ik_controller.cpp @@ -27,7 +27,6 @@ #include "controller_common/common.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "message_transforms/transforms.hpp" #include "pinocchio/algorithm/model.hpp" #include "pinocchio/parsers/urdf.hpp" @@ -69,12 +68,13 @@ auto IKController::on_init() -> controller_interface::CallbackReturn std::ranges::copy(params_.controlled_joints, std::back_inserter(controlled_joints)); std::vector locked_joint_names; - std::ranges::copy_if(model_->names, std::back_inserter(locked_joint_names), [&controlled_joints](const auto & name) { - return std::ranges::find(controlled_joints, name) == controlled_joints.end(); - }); + std::ranges::copy_if( + model_->names, std::back_inserter(locked_joint_names), [&controlled_joints](const auto & name) -> auto { + return std::ranges::find(controlled_joints, name) == controlled_joints.end(); + }); std::vector locked_joints; - std::ranges::transform(locked_joint_names, std::back_inserter(locked_joints), [this](const auto & name) { + std::ranges::transform(locked_joint_names, std::back_inserter(locked_joints), [this](const auto & name) -> auto { return model_->getJointId(name); }); @@ -162,7 +162,6 @@ auto IKController::on_configure(const rclcpp_lifecycle::State & /*previous_state reference_sub_ = get_node()->create_subscription( "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - m2m::transform_message(*msg); reference_.writeFromNonRT(*msg); }); @@ -172,7 +171,6 @@ auto IKController::on_configure(const rclcpp_lifecycle::State & /*previous_state "~/vehicle_state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr msg) { // NOLINT - m2m::transform_message(*msg, "map", "base_link"); vehicle_state_.writeFromNonRT(*msg); }); } @@ -210,13 +208,16 @@ auto IKController::command_interface_configuration() const -> controller_interfa config.type = controller_interface::interface_configuration_type::INDIVIDUAL; config.names.reserve(n_command_interfaces_); - auto format_interface = [](const std::string & name, const std::string & type, const std::string & reference) { + auto format_interface = + [](const std::string & name, const std::string & type, const std::string & reference) -> std::string { return reference.empty() ? std::format("{}/{}", name, type) : std::format("{}/{}/{}", reference, name, type); }; if (use_position_commands_) { std::ranges::transform( - position_interface_names_, std::back_inserter(config.names), [this, &format_interface](const auto & name) { + position_interface_names_, + std::back_inserter(config.names), + [this, &format_interface](const auto & name) -> auto { if (std::ranges::find(free_flyer_pos_dofs_, name) != free_flyer_pos_dofs_.end()) { return format_interface(name, hardware_interface::HW_IF_POSITION, params_.vehicle_reference_controller); } @@ -226,7 +227,9 @@ auto IKController::command_interface_configuration() const -> controller_interfa if (use_velocity_commands_) { std::ranges::transform( - velocity_interface_names_, std::back_inserter(config.names), [this, &format_interface](const auto & name) { + velocity_interface_names_, + std::back_inserter(config.names), + [this, &format_interface](const auto & name) -> auto { if (std::ranges::find(free_flyer_vel_dofs_, name) != free_flyer_vel_dofs_.end()) { return format_interface(name, hardware_interface::HW_IF_VELOCITY, params_.vehicle_reference_controller); } @@ -242,8 +245,9 @@ auto IKController::state_interface_configuration() const -> controller_interface controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - auto insert_interfaces = [&config](const std::vector & interface_names, const std::string & type) { - std::ranges::transform(interface_names, std::back_inserter(config.names), [type](const auto & name) { + auto insert_interfaces = [&config]( + const std::vector & interface_names, const std::string & type) -> void { + std::ranges::transform(interface_names, std::back_inserter(config.names), [type](const auto & name) -> auto { return std::format("{}/{}", name, type); }); }; @@ -309,8 +313,8 @@ auto IKController::update_system_state_values() -> controller_interface::return_ std::ranges::copy(state.begin(), state.begin() + free_flyer_pos_dofs_.size(), position_state_values_.begin()); std::ranges::copy(state.begin() + free_flyer_pos_dofs_.size(), state.end(), velocity_state_values_.begin()); } else { - auto save_states = [](const auto & interfaces, auto out) { - std::ranges::transform(interfaces, out, [](const auto & interface) { + auto save_states = [](const auto & interfaces, auto out) -> auto { + std::ranges::transform(interfaces, out, [](const auto & interface) -> auto { return interface.get_optional().value_or(std::numeric_limits::quiet_NaN()); }); }; @@ -333,17 +337,15 @@ auto IKController::update_system_state_values() -> controller_interface::return_ // transform the states into the appropriate frame and save them geometry_msgs::msg::Pose pose; common::messages::to_msg(position_states, &pose); - m2m::transform_message(pose); std::ranges::copy(common::messages::to_vector(pose), position_state_values_.begin()); geometry_msgs::msg::Twist twist; common::messages::to_msg(velocity_states, &twist); - m2m::transform_message(twist); std::ranges::copy(common::messages::to_vector(twist), velocity_state_values_.begin()); } - auto find_interface = [](const auto & interfaces, const std::string & name, const std::string & type) { - return std::ranges::find_if(interfaces, [&name, &type](const auto & interface) { + auto find_interface = [](const auto & interfaces, const std::string & name, const std::string & type) -> auto { + return std::ranges::find_if(interfaces, [&name, &type](const auto & interface) -> auto { return interface.get_name() == std::format("{}/{}", name, type); }); }; @@ -390,7 +392,6 @@ auto IKController::update_chained_reference_values() -> controller_interface::re // this extra method to transform the values into a frame suitable for pinocchio geometry_msgs::msg::Pose reference_transformed; common::messages::to_msg(reference_interfaces_, &reference_transformed); - m2m::transform_message(reference_transformed); std::ranges::copy(common::messages::to_vector(reference_transformed), reference_interfaces_.begin()); return controller_interface::return_type::OK; } @@ -444,13 +445,11 @@ auto IKController::update_and_write_commands(const rclcpp::Time & /*time*/, cons // transform the solution into the appropriate frame geometry_msgs::msg::Twist twist; common::messages::to_msg({point.velocities.begin(), point.velocities.begin() + free_flyer_vel_dofs_.size()}, &twist); - m2m::transform_message(twist); std::ranges::copy(common::messages::to_vector(twist), point.velocities.begin()); // transform the pose into the appropriate frame geometry_msgs::msg::Pose pose; common::messages::to_msg({point.positions.begin(), point.positions.begin() + free_flyer_pos_dofs_.size()}, &pose); - m2m::transform_message(pose); std::ranges::copy(common::messages::to_vector(pose), point.positions.begin()); if (use_position_commands_) {