Merge pull request #706 from vortexntnu/dp_adaptive_backstepping_controler_to_quaternion

merge control stack from easter testing into main

Author: Q3rkses

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/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]
 
@@ -110,7 +110,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/launch/dp_quat.launch.py

@@ -0,0 +1,72 @@
+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_drone_and_namespace_args,
+    resolve_drone_and_namespace,
+)
+
+
+def launch_setup(context, *args, **kwargs):
+    drone, namespace = resolve_drone_and_namespace(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}.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()
+        + [
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

control/dp_adapt_backs_controller/src/dp_adapt_backs_controller_ros.cpp

@@ -40,9 +40,9 @@ void DPAdaptBacksControllerNode::set_subscribers_and_publisher() {
         vortex::utils::qos_profiles::sensor_data_profile(1)};
     const auto qos_reliable{vortex::utils::qos_profiles::reliable_profile(1)};
 
-    this->declare_parameter<std::string>("topics.guidance.dp");
+    this->declare_parameter<std::string>("topics.guidance.dp_rpy");
     std::string dp_reference_topic =
-        this->get_parameter("topics.guidance.dp").as_string();
+        this->get_parameter("topics.guidance.dp_rpy").as_string();
     guidance_sub_ =
         this->create_subscription<vortex_msgs::msg::ReferenceFilter>(
             dp_reference_topic, qos_sensor_data,
@@ -234,10 +234,7 @@ void DPAdaptBacksControllerNode::publish_tau() {
     tau_msg.wrench.force.x = tau(0);
     tau_msg.wrench.force.y = tau(1);
     tau_msg.wrench.force.z = tau(2);
-
-    // comment out if roll control is not needed
     tau_msg.wrench.torque.x = tau(3);
-
     tau_msg.wrench.torque.y = tau(4);
     tau_msg.wrench.torque.z = tau(5);
 

control/dp_adapt_backs_controller_quat/CMakeLists.txt

@@ -0,0 +1,72 @@
+cmake_minimum_required(VERSION 3.8)
+project(dp_adapt_backs_controller_quat)
+set(CMAKE_CXX_STANDARD 20)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(fmt REQUIRED)
+find_package(spdlog REQUIRED)
+
+
+include_directories(include)
+
+set(LIB_NAME "${PROJECT_NAME}_component")
+
+add_library(${LIB_NAME} SHARED
+  src/dp_adapt_backs_controller.cpp
+  src/dp_adapt_backs_controller_ros.cpp
+  src/dp_adapt_backs_controller_utils.cpp)
+
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  rclcpp_components
+  geometry_msgs
+  Eigen3
+  fmt
+  spdlog
+  vortex_msgs
+  vortex_utils
+  vortex_utils_ros
+)
+
+rclcpp_components_register_node(
+  ${LIB_NAME}
+  PLUGIN "DPAdaptBacksControllerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_export_targets(export_${LIB_NAME})
+
+install(TARGETS ${LIB_NAME}
+  EXPORT export_${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
+ament_package()