merge control stack from easter testing into main

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,

auv_setup/launch/dp_quat.launch.py

@@ -0,0 +1,60 @@
+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 Node
+
+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",
+    )
+
+    return [
+        Node(
+            package="reference_filter_dp_quat",
+            executable="reference_filter_dp_quat_node",
+            name="reference_filter_node",
+            namespace=namespace,
+            parameters=[filter_config, drone_params],
+            output="screen",
+        ),
+        Node(
+            package="dp_adapt_backs_controller_quat",
+            executable="dp_adapt_backs_controller_quat_node",
+            name="dp_adapt_backs_controller_node",
+            namespace=namespace,
+            parameters=[adapt_params, drone_params],
+            output="screen",
+        ),
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        declare_drone_and_namespace_args()
+        + [OpaqueFunction(function=launch_setup)]
+    )

control/dp_adapt_backs_controller_quat/CMakeLists.txt

@@ -0,0 +1,79 @@
+cmake_minimum_required(VERSION 3.8)
+project(dp_adapt_backs_controller_quat)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 20)
+endif()
+
+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(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(tf2 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
+  nav_msgs
+  Eigen3
+  tf2
+  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()