Merge branch 'main' into dev/eskf_fuse_depth

Author: jorgenfj

.pre-commit-config.yaml

@@ -90,7 +90,7 @@ repos:
       - id: ament_cpplint
         name: ament_cpplint
         description: Static code analysis of C/C++ files.
-        entry: ament_cpplint
+        entry: bash -c 'out=$(ament_cpplint "$@" 2>&1); rc=$?; printf "%s\n" "$out" | grep -Ev "^Using|Done processing|No problems found"; exit $rc' --
         language: system
         files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
         args: [

auv_setup/README.md

@@ -10,8 +10,64 @@ 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.
+
+```bash
+ros2 launch auv_setup topside.launch.py --orientation_mode:=your_argument
+```
+
+| Argument | Default | Description |
+|---|---|---|
+| `orientation_mode` |`euler`| Types to choose from `euler` or `quat` |
+
+
+
+**`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
+```
+
+**`adaptive_quat`** — launches `dp_adapt_backs_controller` with `reference_filter_dp` (Quaternion reference filter). Controller params are loaded from `dp_adapt_backs_controller_quat/config/adapt_params_<drone>.yaml`.
+
+```bash
+ros2 launch auv_setup dp_quat.launch.py controller_type:=adaptive drone:=nautilus
+```
+
+#### dp.launch.py or dp_quat.launch.py
+
+Both launch the DP (dynamic positioning) stack, consisting of a reference filter and a controller launched together in a composable node container to limit intra process communication and allow for shared memory.
+
+```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`, `adaptive_quat` 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
+```
+
+**`adaptive_quat`** — launches `dp_adapt_backs_controller` with `reference_filter_dp` (Quaternion reference filter). Controller params are loaded from `dp_adapt_backs_controller_quat/config/adapt_params_<drone>.yaml`.
+
+```bash
+ros2 launch auv_setup dp_quat.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/auv_setup/launch_arg_common.py

@@ -23,3 +23,15 @@ def resolve_drone_and_namespace(context):
     if namespace == "":
         namespace = drone
     return drone, namespace
+
+
+def declare_config_type_arg(default_config_type="physical"):
+    return DeclareLaunchArgument(
+        "config_type",
+        default_value=default_config_type,
+        description="Controller config variant: 'sim' or 'physical'",
+    )
+
+
+def resolve_config_type(context):
+    return LaunchConfiguration("config_type").perform(context)

auv_setup/config/robots/nautilus.yaml

@@ -16,7 +16,7 @@
 /**:
   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
+      center_of_mass: [0.0, 0.0, 0.025] # 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:
@@ -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/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"

auv_setup/description/nautilus.urdf.xacro

@@ -18,14 +18,14 @@
   <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 -->
   <joint name="base_to_dvl" type="fixed">
     <parent link="base_link"/>
     <child link="dvl_link"/>
-    <origin xyz="-0.130 0.003 0.2414" rpy="0.0 0.0 0.0"/>
+    <origin xyz="-0.130 0.003 0.2414" rpy="0.0 0.0 3.14159"/>
   </joint>
 
   <!-- base to pressure sensor -->
@@ -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>

auv_setup/launch/dp.launch.py

@@ -7,13 +7,16 @@
 from launch_ros.descriptions import ComposableNode
 
 from auv_setup.launch_arg_common import (
+    declare_config_type_arg,
     declare_drone_and_namespace_args,
+    resolve_config_type,
     resolve_drone_and_namespace,
 )
 
 
 def launch_setup(context, *args, **kwargs):
     drone, namespace = resolve_drone_and_namespace(context)
+    config_type = resolve_config_type(context)
 
     filter_file_path = os.path.join(
         get_package_share_directory("reference_filter_dp"),
@@ -31,7 +34,7 @@ def launch_setup(context, *args, **kwargs):
     adapt_params = os.path.join(
         get_package_share_directory("dp_adapt_backs_controller"),
         "config",
-        f"adapt_params_{drone}.yaml",
+        f"adapt_params_{drone}_{config_type}.yaml",
     )
 
     container = ComposableNodeContainer(
@@ -68,6 +71,7 @@ def generate_launch_description():
     return LaunchDescription(
         declare_drone_and_namespace_args()
         + [
+            declare_config_type_arg(),
             OpaqueFunction(function=launch_setup),
         ]
     )

auv_setup/launch/dp_quat.launch.py

@@ -0,0 +1,76 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+from auv_setup.launch_arg_common import (
+    declare_config_type_arg,
+    declare_drone_and_namespace_args,
+    resolve_config_type,
+    resolve_drone_and_namespace,
+)
+
+
+def launch_setup(context, *args, **kwargs):
+    drone, namespace = resolve_drone_and_namespace(context)
+    config_type = resolve_config_type(context)
+
+    filter_config = os.path.join(
+        get_package_share_directory("reference_filter_dp_quat"),
+        "config",
+        "reference_filter_params.yaml",
+    )
+
+    drone_params = os.path.join(
+        get_package_share_directory("auv_setup"),
+        "config",
+        "robots",
+        f"{drone}.yaml",
+    )
+
+    adapt_params = os.path.join(
+        get_package_share_directory("dp_adapt_backs_controller_quat"),
+        "config",
+        f"adapt_params_{drone}_{config_type}.yaml",
+    )
+
+    container = ComposableNodeContainer(
+        name="dp_quat_container",
+        namespace=namespace,
+        package="rclcpp_components",
+        executable="component_container_mt",
+        composable_node_descriptions=[
+            ComposableNode(
+                package="reference_filter_dp_quat",
+                plugin="ReferenceFilterNode",
+                name="reference_filter_node",
+                namespace=namespace,
+                parameters=[filter_config, drone_params],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+            ComposableNode(
+                package="dp_adapt_backs_controller_quat",
+                plugin="DPAdaptBacksControllerNode",
+                name="dp_adapt_backs_controller_node",
+                namespace=namespace,
+                parameters=[adapt_params, drone_params],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+        ],
+        output="screen",
+    )
+
+    return [container]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        declare_drone_and_namespace_args()
+        + [
+            declare_config_type_arg(),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )