feat: converted velocity controller lqr to lifecycle node

control/velocity_controller_lqr/README.md

@@ -0,0 +1,45 @@
+## 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/param_velocity_controller_lqr.yaml file:
+
+
+#### Launching the package:
+```bash
+1. inside ros2ws/colcon build --packages-select velocity_controller_lqr
+```
+
+```bash
+2. Inisde ros2ws/: source install/setup.bash
+```
+
+```bash
+3. ros2 launch velocity_controller_lqr velocity_controller_lqr.launch.py
+```
+
+#### 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,6 +25,8 @@
 
       i_weight: 0.5
 
+      dt: 0.1
+
     inertia_matrix: [30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729]
 
     #Clamp parameter

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:
@@ -22,20 +22,13 @@ def generate_launch_description() -> LaunchDescription:
         "param_velocity_controller_lqr.yaml",
     )
 
-    topic_file = os.path.join(
-        get_package_share_directory("auv_setup"),
-        "config",
-        "robots",
-        "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",
-        namespace="orca",
+        namespace="",
         output="screen",
-        parameters=[parameter_file, topic_file],
+        parameters=[parameter_file],
     )
 
     return LaunchDescription([velocity_controller_node])