<feat> implemented Liner Quadratic Regulator

.github/workflows/source-build.yaml

@@ -12,5 +12,6 @@ jobs:
     with:
       ros_distro: 'humble'
       os_name: 'ubuntu-22.04'
-      ref: ${{ github.ref_name }}
+      ref: ${{ github.ref }}
       vcs_repo_file_url: 'https://raw.githubusercontent.com/vortexntnu/vortex-auv/main/ros2.repos'
+      dependency_script: 'requirements.sh'

control/velocity_controller_lqr/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.8)
+project(velocity_controller_lqr)
+
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
+
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  scripts/velocity_controller_lqr_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  set(_pytest_tests
+    tests/test_velocity_controller_lqr.py
+  )
+  foreach(_test_path ${_pytest_tests})
+    get_filename_component(_test_name ${_test_path} NAME_WE)
+    ament_add_pytest_test(${_test_name} ${_test_path}
+      APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+      WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+    )
+  endforeach()
+endif()
+
+ament_package()

control/velocity_controller_lqr/README.txt

@@ -0,0 +1 @@
+This package contains the velocity controller for the AUV Orca. The controller utilizes an LQR optimal controller (imported from the python control library), and controls pitch, yaw and surge. The controller is meant to traverse larger distances.

control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml

@@ -0,0 +1,32 @@
+velocity_controller_lqr_node:
+  ros__parameters:
+    dt: 0.1
+
+    topics:
+      odom_topic: /orca/odom
+      twist_topic: /dvl/twist
+      pose_topic: /dvl/pose
+      guidance_topic: /guidance/los
+      thrust_topic: /thrust/wrench_input
+      softwareoperation_topic: /softwareOperationMode
+      killswitch_topic: /softwareKillSwitch
+
+    LQR_params:
+      q_surge: 75
+      q_pitch: 175
+      q_yaw: 175
+
+      r_surge: 0.3
+      r_pitch: 0.4
+      r_yaw: 0.4
+
+      i_surge: 0.3
+      i_pitch: 0.4
+      i_yaw: 0.3
+
+      i_weight: 0.5
+
+    inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729]
+
+    #Clamp parameter
+    max_force: 99.5

control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py

@@ -0,0 +1,33 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Generates a launch description for the velocity_controller_lqr node.
+
+    This function creates a ROS 2 launch description that includes the
+    velocity_controller_lqr node. The node is configured to use the
+    parameters specified in the 'params_velocity_controller_lqr.yaml' file.
+
+    Returns:
+        LaunchDescription: A ROS 2 launch description containing the
+        velocity_controller_lqr node.
+    """
+    parameter_file = os.path.join(
+        get_package_share_directory("velocity_controller_lqr"),
+        "config",
+        "param_velocity_controller_lqr.yaml",
+    )
+
+    velocity_controller_node = Node(
+        package="velocity_controller_lqr",
+        executable="velocity_controller_lqr_node.py",
+        name="velocity_controller_lqr_node",
+        output="screen",
+        parameters=[parameter_file],
+    )
+
+    return LaunchDescription([velocity_controller_node])

control/velocity_controller_lqr/package.xml

@@ -0,0 +1,24 @@
+<?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>velocity_controller_lqr</name>
+  <version>1.0.0</version>
+  <description>Velocity controller package for the AUV Orca</description>
+  <maintainer email="[email protected]">cyprian</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>python-control-pip</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_cmake_pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

control/velocity_controller_lqr/requirements.txt

@@ -0,0 +1 @@
+numpy<1.25.0