Robotic-Decision-Making-Lab · evan-palmer · Apr 27, 2025 · Sep 25, 2024 · Feb 12, 2025 · Mar 19, 2025
diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile
@@ -71,8 +71,7 @@ WORKDIR $USER_WORKSPACE
 
 # Install debugging/linting Python packages
 RUN pip install \
-    pre-commit \
-    mypy
+    pre-commit
 
 # Install debugging/linting C++ packages
 RUN sudo apt-get -q update \

diff --git a/.dockerignore b/.dockerignore
@@ -3,11 +3,14 @@
 *
 
 # Except the following
-
+!auv_control_demos
+!auv_control_msgs
 !auv_controllers
-!velocity_controllers
+!controller_common
+!ik_solvers
 !thruster_allocation_matrix_controller
 !thruster_controllers
-!auv_control_msgs
-!auv_control_demos
+!topic_sensors
+!velocity_controllers
+!whole_body_controllers
 !ros2.repos
diff --git a/.github/ISSUE_TEMPLATE/bug-report.yml b/.github/ISSUE_TEMPLATE/bug-report.yml
@@ -1,7 +1,8 @@
 name: Bug Report
 description: Report a bug.
-title: "[BUG]: <Please write a descriptive title after the '[BUG]: ' prefix>"
-labels: [bug, needs triage]
+title: "<Please write a descriptive title>"
+labels: [needs triage]
+type: Bug
 
 body:
   - type: markdown

diff --git a/.github/ISSUE_TEMPLATE/documentation.yml b/.github/ISSUE_TEMPLATE/documentation.yml
@@ -1,7 +1,8 @@
 name: Documentation Improvement
 description: Report an issue related to the project documentation.
-title: "[DOC]: <Please write a descriptive title after the '[DOC]: ' prefix>"
-labels: [documentation, needs triage]
+title: "<Please write a descriptive title>"
+labels: [needs triage]
+type: Documentation
 
 body:
   - type: markdown

diff --git a/.github/ISSUE_TEMPLATE/feature-request.yml b/.github/ISSUE_TEMPLATE/feature-request.yml
@@ -1,7 +1,8 @@
 name: Feature Request
 description: Suggest a new idea for the project.
-title: "[FEATURE]: <Please write a descriptive title after the '[FEATURE]: ' prefix>"
-labels: [enhancement, needs triage]
+title: "<Please write a descriptive title>"
+labels: [needs triage]
+type: Feature
 
 body:
   - type: markdown

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -28,7 +28,7 @@ jobs:
         uses: ros-industrial/industrial_ci@master
         env:
           ROS_DISTRO: ${{ matrix.env.ROS_DISTRO }}
-          CXXFLAGS: -Wall -Wextra -Wpedantic
+          CXXFLAGS: -Wall -Wextra -Wpedantic -std=c++20
           CLANG_TIDY: true
           UPSTREAM_WORKSPACE: ros2.repos
           AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -22,7 +22,7 @@ repos:
     - id: gersemi
 
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.4.0
+    rev: v5.0.0
     hooks:
       - id: check-added-large-files
       - id: check-case-conflict

diff --git a/.ruff.toml b/.ruff.toml
@@ -1 +1 @@
-target-version = "py310"
+target-version = "py312"
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
@@ -11,7 +11,7 @@
       "defines": [],
       "compilerPath": "/usr/bin/clang",
       "cStandard": "c99",
-      "cppStandard": "c++20",
+      "cppStandard": "c++23",
       "intelliSenseMode": "linux-clang-x64"
     }
   ],

diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -3,10 +3,11 @@
     "*.repos": "yaml",
     "*.world": "xml",
     "*.xacro": "xml",
-    "*.srdf": "xml",
     "*.rviz": "yaml",
     "*.config": "xml",
-    "*.sdf": "xml"
+    "*.sdf": "xml",
+    "limits": "cpp",
+    "format": "cpp"
   },
   "terminal.integrated.defaultProfile.linux": "bash",
   "terminal.integrated.profiles.linux": {

diff --git a/README.md b/README.md
@@ -1,19 +1,15 @@
 # auv_controllers
 
-auv_controllers is a collection of chainable controllers for autonomous
-underwater vehicles (AUVs) implemented using ros2_control. The controllers have
+auv_controllers is a collection of controllers for autonomous underwater
+vehicles (AUVs) implemented using ros2_control. The controllers have
 been designed to support the complete AUV control hierarchy and to enable
 benchmarking against other commonly-used control algorithms.
 
-> [!NOTE]
-> If you are interested in adding your own controller to this project, please
-> consider submitting a [pull request](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/pulls)!
-
 ## Installation
 
 auv_controllers is currently supported on Linux and is available for the ROS 2
-Rolling, Jazzy, Iron, and Humble distributions. To install auv_controllers,
-first clone this project to the `src` directory of your ROS workspace:
+Rolling and Jazzy distributions. To install auv_controllers, first clone this
+project to the `src` directory of your ROS workspace:
 
 ```bash
 git clone git@github.com:Robotic-Decision-Making-Lab/auv_controllers.git

diff --git a/auv_control_demos/CHANGELOG.md b/auv_control_demos/CHANGELOG.md
@@ -0,0 +1,6 @@
+# Changelog for package auv_control_demos
+
+## 0.1.0 (2025-04-27)
+
+- Updates the individual_controller and chained_controllers demos to use the
+correct topic names
diff --git a/auv_control_demos/CMakeLists.txt b/auv_control_demos/CMakeLists.txt
@@ -1,12 +1,12 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.23)
 project(auv_control_demos)
 
 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)
-endforeach()
+find_package(rclpy REQUIRED)
+
+ament_export_dependencies(rclpy)
 
 install(
     DIRECTORY

diff --git a/auv_control_demos/chained_controllers/README.md b/auv_control_demos/chained_controllers/README.md
@@ -88,7 +88,7 @@ applies a PWM command to the hardware interface.
    following command:
 
    ```bash
-   ros2 topic pub /integral_sliding_mode_controller/system_state geometry_msgs/msg/Twist
+   ros2 topic pub /integral_sliding_mode_controller/system_state nav_msgs/msg/Odometry
    ```
 
 5. The ISMC accepts reference commands sent over a topic or a reference

diff --git a/auv_control_demos/individual_controller/README.md b/auv_control_demos/individual_controller/README.md
@@ -1,7 +1,6 @@
 # Example 1: Individual Controller
 
-This example uses the [integral sliding mode controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) to demonstrate how
-to launch a single controller.
+This example uses the [integral sliding mode controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) to demonstrate how to launch a single controller.
 
 ## Tutorial Steps
 
@@ -62,7 +61,7 @@ to launch a single controller.
    topic-based interface:
 
    ```bash
-   ros2 topic pub /integral_sliding_mode_controller/system_state geometry_msgs/msg/Twist
+   ros2 topic pub /integral_sliding_mode_controller/system_state nav_msgs/msg/Odometry
    ```
 
 5. The ISMC accepts reference commands sent via a topic or the controller's
@@ -80,7 +79,7 @@ to launch a single controller.
    ros2 topic echo /integral_sliding_mode_controller/status
    ```
 
-   Should yield an output similar to the following:
+   should yield an output similar to the following:
 
    ```bash
    header:

diff --git a/auv_control_demos/package.xml b/auv_control_demos/package.xml
@@ -1,10 +1,10 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
+
   <name>auv_control_demos</name>
-  <version>0.0.1</version>
-  <description>Example package that includes demos for using auv_controllers in individual and
-    chained modes</description>
+  <version>0.1.0</version>
+  <description>Example package that includes demos for using auv_controllers in individual and chained modes</description>
 
   <maintainer email="mitchcol@oregonstate.edu">Colin Mitchell</maintainer>
   <maintainer email="everardo.a.gonzalez@gmail.com">Everardo Gonzalez</maintainer>

diff --git a/auv_control_msgs/CHANGELOG.md b/auv_control_msgs/CHANGELOG.md
@@ -0,0 +1,6 @@
+# Changelog for package auv_control_msgs
+
+## 0.1.0 (2025-04-27)
+
+- Implements the IKControllerStateStamped message to support the new IK
+controller
diff --git a/auv_control_msgs/CMakeLists.txt b/auv_control_msgs/CMakeLists.txt
@@ -1,14 +1,17 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.23)
 project(auv_control_msgs)
 
 find_package(ament_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(trajectory_msgs REQUIRED)
 
 rosidl_generate_interfaces(auv_control_msgs
   "msg/MultiActuatorStateStamped.msg"
-  DEPENDENCIES builtin_interfaces std_msgs
+  "msg/IKControllerStateStamped.msg"
+  DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs
 )
 
 ament_package()
diff --git a/auv_control_msgs/msg/IKControllerStateStamped.msg b/auv_control_msgs/msg/IKControllerStateStamped.msg
@@ -0,0 +1,20 @@
+std_msgs/Header header
+
+# The name of the IK solver used by the controller
+string solver_name
+
+# Position DoF names, e.g., joint or axis names.
+string[] position_joint_names
+
+# Velocity DoF names.
+string[] velocity_joint_names
+
+# The reference end effector pose.
+geometry_msgs/Pose reference
+
+# Time between two consecutive updates/execution of the control law.
+# This is often used for integration of the solution to determine the desired positions.
+float64 time_step
+
+# The IK solver solution.
+trajectory_msgs/JointTrajectoryPoint solution
diff --git a/auv_control_msgs/package.xml b/auv_control_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>auv_control_msgs</name>
-  <version>0.0.1</version>
+  <version>0.1.0</version>
   <description>Custom messages for AUV controllers</description>
 
   <maintainer email="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>
@@ -18,10 +18,9 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
   <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>trajectory_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>

diff --git a/auv_controllers/CHANGELOG.md b/auv_controllers/CHANGELOG.md
@@ -0,0 +1,11 @@
+# Changelog for package auv_controllers
+
+## 0.1.0 (2025-04-27)
+
+- Adds the adaptive integral terminal sliding mode controller
+- Adds the task priority IK solver
+- Adds the IK whole-body controller
+- Adds the odom topic sensor
+- Adds the controller_common package
+- Adds the Gazebo passthrough thruster controller
+- Adds the thruster rotation rate controller
diff --git a/auv_controllers/CMakeLists.txt b/auv_controllers/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.23)
 project(auv_controllers)
 
 find_package(ament_cmake REQUIRED)

diff --git a/auv_controllers/package.xml b/auv_controllers/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
 
   <name>auv_controllers</name>
-  <version>0.0.1</version>
+  <version>0.1.0</version>
   <description>Meta package for auv_controllers</description>
 
   <maintainer email="evanp922@gmail.com">Evan Palmer</maintainer>
@@ -16,10 +16,14 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <exec_depend>auv_control_msgs</exec_depend>
+  <exec_depend>controller_common</exec_depend>
+  <exec_depend>ik_solvers</exec_depend>
   <exec_depend>thruster_allocation_matrix_controller</exec_depend>
   <exec_depend>thruster_controllers</exec_depend>
+  <exec_depend>topic_sensors</exec_depend>
   <exec_depend>velocity_controllers</exec_depend>
-  <exec_depend>auv_control_msgs</exec_depend>
+  <exec_depend>whole_body_controllers</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/controller_common/CHANGELOG.md b/controller_common/CHANGELOG.md
@@ -0,0 +1,5 @@
+# Changelog for package controller_common
+
+## 0.1.0 (2025-04-27)
+
+- Ports reset message functions and error calculation to a common API
diff --git a/controller_common/CMakeLists.txt b/controller_common/CMakeLists.txt
@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.23)
+project(controller_common)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+include(GNUInstallDirs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+
+add_library(controller_common SHARED)
+target_sources(
+    controller_common
+    PRIVATE src/common.cpp
+    PUBLIC
+        FILE_SET HEADERS
+        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
+        FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/controller_common/common.hpp
+)
+target_compile_features(controller_common PUBLIC cxx_std_23)
+target_link_libraries(
+    controller_common
+    PUBLIC rclcpp::rclcpp ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS}
+)
+
+install(
+    TARGETS controller_common
+    EXPORT export_controller_common
+    RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+    LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+    ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+    FILE_SET HEADERS
+)
+
+ament_export_targets(export_controller_common HAS_LIBRARY_TARGET)
+ament_export_dependencies("rclcpp" "geometry_msgs" "nav_msgs")
+
+ament_package()
diff --git a/controller_common/LICENSE b/controller_common/LICENSE
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.