feat: converted velocity controller lqr to lifecycle node

control/velocity_controller_lqr/README.md

@@ -0,0 +1,47 @@
+## Overview:
+---
+Contains the LQR controller package 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.
+
+#### Tuning of Parameters:
+ To tune parameters look at the [config file](https://github.com/vortexntnu/vortex-auv/blob/velocity-controller-to-lifecycle/control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml) file:
+
+
+#### Launching the package:
+1. Navigate to your ros2ws directory then (change your own path after cd or skip first line entirely),
+```bash
+cd ~/ros2ws/ &&
+colcon build --packages-select velocity_controller_lqr &&
+source install/setup.bash
+```
+
+2. Then simply run the node via the launch file
+```bash
+ ros2 launch velocity_controller_lqr velocity_controller_lqr.launch.py
+```
+
+3. You will notice that not much is currently happening, however that is because this is a lifecycle node and you can now transition between the node states through the terminal.
+
+#### Transitioning between states manually:
+The ROS2 node is implemented using lifecycle nodes, which are managed externally by a lifecycle manager i.e a finite state machine. If you want to manually test the node do the following:
+
+**From Unconfigured ---> Inactive**
+```bash
+ros2 lifecycle set /velocity_controller_lqr_node configure
+```
+
+**From Configured ---> Active**
+```bash
+ros2 lifecycle set /velocity_controller_lqr_node activate
+```
+
+**From Active ---> Inactive**
+```bash
+ros2 lifecycle set /velocity_controller_lqr_node deactivate
+```
+
+For the full state diagram you can refer to the figure below, sourced from the official ROS2 Documentation:
+![image info](./figures/ros2_transition_diagram.png)
+
+
+### Theory
+---

control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml

@@ -1,6 +1,14 @@
 /**:
   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
@@ -17,7 +25,6 @@
 
       i_weight: 0.5
 
-    inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729]
+    dt: 0.1
 
-    #Clamp parameter
-    max_force: 99.5
+    inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729]

control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py

@@ -2,7 +2,7 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import LifecycleNode
 
 
 def generate_launch_description() -> LaunchDescription:
@@ -29,7 +29,7 @@ def generate_launch_description() -> LaunchDescription:
         "orca.yaml",
     )
 
-    velocity_controller_node = Node(
+    velocity_controller_node = LifecycleNode(
         package="velocity_controller_lqr",
         executable="velocity_controller_lqr_node.py",
         name="velocity_controller_lqr_node",

control/velocity_controller_lqr/package.xml

@@ -2,7 +2,7 @@
 <?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>
+  <version>0.6.9</version>
   <description>Velocity controller package for the AUV Orca</description>
   <maintainer email="cyprian.github@gmail.com">cyprian</maintainer>
   <license>MIT</license>