Easter testing 2026

.gitignore

@@ -59,3 +59,8 @@ qtcreator-*
 .#*
 
 # End of https://www.gitignore.io/api/ros
+control/pid_controller_dp/test/test_main.cpp
+control/pid_controller_dp/test/test_pid_basic.cpp
+control/pid_controller_dp/test/test_pid_controller.cpp
+control/pid_controller_dp/test/test_type_casting.cpp
+scripts/ci_install_dependencies.sh

.pre-commit-config.yaml

@@ -12,6 +12,8 @@
 #
 # See https://github.com/pre-commit/pre-commit
 
+exclude: (^|/)(test|tests)/.*
+
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks

auv_setup/README.md

@@ -10,8 +10,36 @@ The config folder contains physical parameters related to the AUV and the enviro
 * thrusters: Thruster configs for different thruster types
 
 ### Launch
-This package contains a launchfile for each specific AUV. Additionally the topside.launch file is to
-be used on the topside computer that the joystick is connected to, for ROV operations.
+
+This package contains launchfiles for each specific AUV. Additionally `topside.launch.py` is used on the topside computer that the joystick is connected to, for ROV operations.
+
+#### dp.launch.py
+
+Launches the DP (dynamic positioning) stack — a reference filter and a controller — as a composable node container.
+
+```bash
+ros2 launch auv_setup dp.launch.py
+```
+
+| Argument | Default | Description |
+|---|---|---|
+| `drone` | `nautilus` | Drone model — loads `auv_setup/config/robots/<drone>.yaml` |
+| `namespace` | `<drone>` | ROS namespace. Defaults to the drone name if left empty |
+| `controller_type` | `adaptive` | Controller to use: `adaptive` or `pid` |
+
+**`adaptive`** — launches `dp_adapt_backs_controller` with `reference_filter_dp` (Euler/RPY reference filter). Controller params are loaded from `dp_adapt_backs_controller/config/adapt_params_<drone>.yaml`.
+
+```bash
+ros2 launch auv_setup dp.launch.py controller_type:=adaptive drone:=nautilus
+```
+
+**`pid`** — launches `pid_controller_dp` with `reference_filter_dp_quat` (quaternion reference filter). Controller params are loaded from `pid_controller_dp/config/pid_params.yaml`.
+
+```bash
+ros2 launch auv_setup dp.launch.py controller_type:=pid drone:=nautilus
+```
+
+> When using the joystick to send references, make sure `orientation_mode` in `joystick_interface_auv` matches the controller: use `euler` with `adaptive` and `quat` with `pid`.
 
 ### Description
 The description folder contains the URDF and xacro files for the AUVs. The main description launch file is drone_description.launch.py, which makes all static transforms available to the ros graph.

auv_setup/config/robots/nautilus.yaml

@@ -16,8 +16,8 @@
 /**:
   ros__parameters:
     physical:
-      center_of_mass: [0.0, 0.0, 0.01] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch
-      mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998]
+      center_of_mass: [0.0, 0.0, 0.05] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch
+      mass_matrix: [55.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 55.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 55.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998]
       # 6x6 mass_inertia_matrix
     propulsion:
       dofs:
@@ -53,22 +53,22 @@
             0.00000, # Heave
           ]
         thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0)
-          0.413892,
-          0.140095,
-          -0.163904,
-          -0.413892,
-          -0.413892,
-          -0.163904,
-          0.140095,
-          0.413892, # x-positions of the thrusters
+          0.45015,
+          0.24060,
+          -0.22970,
+          -0.43861,
+          -0.43861,
+          -0.22970,
+          0.240600,
+          0.450150, # x-positions of the thrusters
+          0.305680,
           0.313022,
           0.313022,
-          0.313022,
-          0.313022,
-          -0.313022,
+          0.305680,
+          -0.305680,
           -0.313022,
           -0.313022,
-          -0.313022, # y-positions of the thrusters
+          -0.305680, # y-positions of the thrusters
           0.021736,
           0.021736,
           0.021736,
@@ -80,8 +80,8 @@
         ]
 
         constraints:
-          max_force: 40.0
-          min_force: -40.0
+          max_force: 39.0
+          min_force: -41.0
           input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0]
           slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0]
 
@@ -90,6 +90,10 @@
 
         thrust_update_rate: 100.0 # [Hz]
         watchdog_timeout: 1.0 # [s]
+        thruster_to_pin_mapping: [4, 5, 7, 6, 0, 1, 2, 3] # I.e. if thruster_to_pin = [ 7,  6,  5,  4,  3,  2,  1,  0 ] then thruster 0 is pin 1 etc..
+        thruster_direction: [-1, -1, 1, 1, -1, -1, 1, 1] # Disclose during thruster mapping (+/- 1)
+        thruster_PWM_min: [1050, 1050, 1050, 1050, 1050, 1050, 1050, 1050] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100]
+        thruster_PWM_max: [1950, 1950, 1950, 1950, 1950, 1950, 1950, 1950] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900]
 
     topics:
       wrench_input: "wrench_input"
@@ -110,7 +114,7 @@
       waypoint: "waypoint"
       guidance:
         los: "guidance/los"
-        dp: "guidance/dp"
+        dp_rpy: "guidance/dp_rpy"
         dp_quat: "guidance/dp_quat"
       dvl_twist: "dvl/twist"
       dvl_altitude: "dvl/altitude"

auv_setup/config/robots/nautilus_bak.yaml

@@ -0,0 +1,140 @@
+# This file defines parameters specific to Nautilus.
+# When looking at the AUV from above, the thruster placement is:
+#
+#        front
+#       |======|
+#  |=7↗=|      |=0↖=|
+#  |    |      |    |
+#  | 6• |      | 1• |
+#  |    |      |    |
+#  |    |      |    |
+#  | 5• |      | 2• |
+#  |    |      |    |
+#  |=4↘=|==||==|=3↙=|
+#
+
+/**:
+  ros__parameters:
+    physical:
+      center_of_mass: [0.0, 0.0, 0.1] # CO is aligned with CM Position (x,y) in meters (M), small cg offset in z to keep drone naturally stable in roll/pitch
+      mass_matrix: [53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 53.7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 11.0628, 1.086, -3.17502, 0.0, 0.0, 0.0, 1.086, 23.1128, 0.1025, 0.0, 0.0, 0.0, -3.17502, 0.1025, 26.23998]
+      # 6x6 mass_inertia_matrix
+    propulsion:
+      dofs:
+        num: 6
+      dimensions:
+        num: 3
+      thrusters:
+        num: 8
+        thruster_force_direction: [ # Direction of forces X,Y,Z, same logic as thruster_position thruster0 produces thrust in (X0,Y0,Z0) and ||(X0,Y0,Z0)|| = 1
+            0.70711,
+            0.00000,
+            0.00000,
+            -0.70711,
+            -0.70711,
+            0.00000,
+            0.00000,
+            0.70711, # Surge
+            -0.70711,
+            0.00000,
+            0.00000,
+            -0.70711,
+            0.70711,
+            0.00000,
+            0.00000,
+            0.70711, # Sway
+            0.00000,
+            1.00000,
+            1.00000,
+            0.00000,
+            0.00000,
+            1.00000,
+            1.00000,
+            0.00000, # Heave
+          ]
+        thruster_position: [ # Position (x0,x1 ... x7,y1,y2, ...,y7,z1,z2, ... ,z7) in meters (M). i.e thruster0 has position (x0,y0,z0)
+          0.45015,
+          0.24060,
+          -0.22970,
+          -0.43861,
+          -0.43861,
+          -0.22970,
+          0.240600,
+          0.450150, # x-positions of the thrusters
+          0.305680,
+          0.313022,
+          0.313022,
+          0.305680,
+          -0.305680,
+          -0.313022,
+          -0.313022,
+          -0.305680, # y-positions of the thrusters
+          0.021736,
+          0.021736,
+          0.021736,
+          0.021736,
+          0.021736,
+          0.021736,
+          0.021736,
+          0.021736, # z-positions of the thrusters
+        ]
+
+        constraints:
+          max_force: 40.0
+          min_force: -40.0
+          input_matrix_weights: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0]
+          slack_matrix_weights: [2000.0,2000.0,2000.0,2000.0,2000.0,2000.0]
+
+        rate_of_change:
+          max: 1 # Maximum rate of change in newton per second for a thruster
+
+        thrust_update_rate: 100.0 # [Hz]
+        watchdog_timeout: 1.0 # [s]
+        thruster_to_pin_mapping: [4, 5, 7, 6, 0, 1, 2, 3] # I.e. if thruster_to_pin = [ 7,  6,  5,  4,  3,  2,  1,  0 ] then thruster 0 is pin 1 etc..
+        thruster_direction: [-1, -1, 1, 1, -1, -1, 1, 1] # Disclose during thruster mapping (+/- 1)
+        thruster_PWM_min: [1050, 1050, 1050, 1050, 1050, 1050, 1050, 1050] # Minimum PWM value, Recommended: [1100, 1100, 1100, 1100, 1100, 1100, 1100, 1100]
+        thruster_PWM_max: [1950, 1950, 1950, 1950, 1950, 1950, 1950, 1950] # Maximum PWM value, Recommended: [1900, 1900, 1900, 1900, 1900, 1900, 1900, 1900]
+
+    topics:
+      wrench_input: "wrench_input"
+      thruster_forces: "thruster_forces"
+      pwm_output: "pwm"
+      current: "power_sense_module/current"
+      voltage: "power_sense_module/voltage"
+      pressure: "pressure"
+      temperature: "temperature"
+      joy: "joy"
+      pose: "pose"
+      twist: "twist"
+      odom: "odom"
+      operation_mode: "operation_mode"
+      killswitch: "killswitch"
+      reference_pose: "reference_pose"
+      landmarks: "landmarks"
+      waypoint: "waypoint"
+      guidance:
+        los: "guidance/los"
+        dp_rpy: "guidance/dp_rpy"
+        dp_quat: "guidance/dp_quat"
+      dvl_twist: "dvl/twist"
+      dvl_altitude: "dvl/altitude"
+      imu: "imu/data_raw"
+      sonar_info: "fls/sonar_info"
+      pressure_sensor: "pressure_sensor"
+      gripper_servos: "gripper_servos"
+
+
+    action_servers:
+      reference_filter: "reference_filter"
+      los: "los_guidance"
+      landmark_polling: "landmark_polling"
+      landmark_convergence: "landmark_convergence"
+      waypoint_manager: "waypoint_manager"
+
+    services:
+      set_operation_mode: "set_operation_mode"
+      set_killswitch: "set_killswitch"
+      toggle_killswitch: "toggle_killswitch"
+      get_operation_mode: "get_operation_mode"
+      waypoint_addition: "waypoint_addition"
+      start_mission: "start_mission"

auv_setup/config/robots/orca.yaml

@@ -134,7 +134,7 @@
       waypoint: "waypoint"
       guidance:
         los: "guidance/los"
-        dp: "guidance/dp"
+        dp_rpy: "guidance/dp_rpy"
         dp_quat: "guidance/dp_quat"
       fsm:
         active_controller: "fsm/active_controller"
@@ -156,6 +156,7 @@
       set_killswitch: "set_killswitch"
       toggle_killswitch: "toggle_killswitch"
       get_operation_mode: "get_operation_mode"
+      los_mode: "set_los_mode"
       waypoint_addition: "waypoint_addition"
       start_mission: "start_mission"
 

auv_setup/description/nautilus.urdf.xacro

@@ -18,7 +18,7 @@
   <joint name="base_to_imu" type="fixed">
     <parent link="base_link"/>
     <child link="imu_link"/>
-    <origin xyz="0.0057 0.0 0.0827" rpy="0.0 0.0 0.0"/>
+    <origin xyz="-0.1 -0.001 0.085" rpy="0.0 0.0 0.0"/>
   </joint>
 
   <!-- base to dvl -->
@@ -46,7 +46,7 @@
   <joint name="base_to_downwards_camera" type="fixed">
     <parent link="base_link"/>
     <child link="downwards_camera_link"/>
-    <origin xyz="0.283615 -0.085021 0.094186" rpy="-1.5708 -1.5708 -1.5708"/>
+    <origin xyz="0.283615 -0.158 0.12" rpy="-1.5708 -1.5708 -1.5708"/>
   </joint>
   <xacro:flir_blackfly_s frame="downwards_camera_link" base="downwards_camera" name="flir_blackfly"/>
 
@@ -66,7 +66,7 @@
   <joint name="shoulder_joint" type="fixed">
     <parent link="base_link"/>
     <child link="shoulder"/>
-    <origin xyz="0.282589 0.07822 0.115803" rpy="0.0 0.0 0.0"/>
+    <origin xyz="0.282589 0.028 0.115803" rpy="0.0 0.0 0.0"/>
   </joint>
 
   <link name="arm"></link>