feat: converted velocity controller lqr to lifecycle node

control/velocity_controller_lqr/README.md

@@ -3,21 +3,23 @@
 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:
+ 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
-1. inside ros2ws/colcon build --packages-select velocity_controller_lqr
+cd ~/ros2ws/ &&
+colcon build --packages-select velocity_controller_lqr &&
+source install/setup.bash
 ```
 
+2. Then simply run the node via the launch file
 ```bash
-2. Inisde ros2ws/: source install/setup.bash
+ ros2 launch velocity_controller_lqr velocity_controller_lqr.launch.py
 ```
 
-```bash
-3. 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:

control/velocity_controller_lqr/config/param_velocity_controller_lqr.yaml

@@ -1,14 +1,14 @@
 /**:
   ros__parameters:
 
-    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
+    # 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
@@ -25,9 +25,6 @@
 
       i_weight: 0.5
 
-      dt: 0.1
+    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
-    max_force: 99.5

control/velocity_controller_lqr/launch/velocity_controller_lqr.launch.py

@@ -22,13 +22,20 @@ 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 = LifecycleNode(
         package="velocity_controller_lqr",
         executable="velocity_controller_lqr_node.py",
         name="velocity_controller_lqr_node",
-        namespace="",
+        namespace="orca",
         output="screen",
-        parameters=[parameter_file],
+        parameters=[parameter_file, topic_file],
     )
 
     return LaunchDescription([velocity_controller_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>

control/velocity_controller_lqr/scripts/velocity_controller_lqr_node.py

@@ -4,7 +4,7 @@
 from geometry_msgs.msg import (
     PoseWithCovarianceStamped,
     TwistWithCovarianceStamped,
-    Wrench,
+    WrenchStamped,
 )
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.lifecycle import LifecycleNode
@@ -46,7 +46,7 @@ def __init__(self):
         # --------------------------- SUBSCRIBERS --------------------------
         self.pose_subscriber = None
         self.twist_subscriber = None
-        self.operationmode_subscriber = None
+        self.operation_mode_subscriber = None
         self.killswitch_subscriber = None
         self.guidance_subscriber = None
         # ------------------------ CONTROLLER MODES ------------------------
@@ -56,21 +56,22 @@ def __init__(self):
         self.publisherLQR = None
         # ----------------------------- TIMERS -----------------------------
         self.control_timer = None
-
         # ------------------ ROS2 PARAMETERS AND CONTROLLER ------------------
         self.get_and_reshape_inertia_matrix()
         self.controller = LQRController(self.lqr_params, self.inertia_matrix)
 
     def on_configure(self, previous_state: LifecycleState) -> TransitionCallbackReturn:
-        self.declare_parameters()
-        self.get_parameters()
+        self.declare_params()
+        self.get_logger().info("1")
+        self.get_params()
+        self.get_logger().info("2")
         # -------------------------- GET ALL TOPICS -------------------------
         (
             pose_topic,
             twist_topic,
             guidance_topic,
             thrust_topic,
-            softwareoperation_topic,
+            software_operation_topic,
             killswitch_topic,
         ) = self.get_topics()
 
@@ -88,9 +89,9 @@ def on_configure(self, previous_state: LifecycleState) -> TransitionCallbackRetu
             qos_profile=self.best_effort_qos,
         )
 
-        self.operationmode_subscriber = self.create_subscription(
+        self.operation_mode_subscriber = self.create_subscription(
             String,
-            softwareoperation_topic,
+            software_operation_topic,
             self.operation_callback,
             qos_profile=self.reliable_qos,
         )
@@ -110,7 +111,7 @@ def on_configure(self, previous_state: LifecycleState) -> TransitionCallbackRetu
 
         # ---------------------------- PUBLISHERS ----------------------------
         self.publisherLQR = self.create_lifecycle_publisher(
-            Wrench, thrust_topic, self.reliable_qos
+            WrenchStamped, thrust_topic, self.reliable_qos
         )
 
         # ------------------------------ TIMERS ------------------------------
@@ -128,7 +129,7 @@ def on_activate(self, previous_state: LifecycleState) -> TransitionCallbackRetur
     def on_deactivate(self, previous_state: LifecycleState) -> TransitionCallbackReturn:
         self.control_timer.cancel()
         self.controller.reset_controller()
-        return super().on_activate(previous_state)
+        return super().on_deactivate(previous_state)
 
     def on_cleanup(self, previous_state: LifecycleState) -> TransitionCallbackReturn:
         self.destroy_publisher(self.publisherLQR)
@@ -144,59 +145,65 @@ def get_topics(self) -> None:
         """Get the topics from the parameter server.
 
         Returns:
-        odom_topic: str: The topic for accessing the odometry data from the parameter file
-        twist_topic: str: The topic for accessing the twist data from the parameter file
         pose_topic: str: The topic for accessing the pose data from the parameter file
+        twist_topic: str: The topic for accessing the twist data from the parameter file
         guidance_topic: str: The topic for accessing the guidance data the parameter file
         thrust_topic: str: The topic for accessing the thrust data from the parameter file
-        """
-        self.declare_parameter("topics.pose_topic", "/dvl/pose")
-        self.declare_parameter("topics.twist_topic", "/dvl/twist")
-        self.declare_parameter("topics.guidance_topic", "/guidance/los")
-        self.declare_parameter("topics.thrust_topic", "/thrust/wrench_input")
-        self.declare_parameter(
-            "topics.softwareoperation_topic", "/softwareOperationMode"
-        )
-        self.declare_parameter("topics.killswitch_topic", "/softwareKillSwitch")
+        software_operation_mode_topic: str: The topic for accessing the operation mode from the parameter file
+        killswitch_topic: str: The topic for accessing the killswitch bool from the parameter file
 
-        pose_topic = self.get_parameter("topics.pose_topic").value
-        twist_topic = self.get_parameter("topics.twist_topic").value
-        guidance_topic = self.get_parameter("topics.guidance_topic").value
-        thrust_topic = self.get_parameter("topics.thrust_topic").value
-        softwareoperation_topic = self.get_parameter(
-            "topics.softwareoperation_topic"
-        ).value
-        killswitch_topic = self.get_parameter("topics.killswitch_topic").value
+        """
+        self.declare_parameter("topics.pose", "_")
+        self.declare_parameter("topics.twist", "_")
+        self.declare_parameter("topics.guidance.los", "_")
+        self.declare_parameter("topics.wrench_input", "_")
+        self.declare_parameter("topics.operation_mode", "_")
+        self.declare_parameter("topics.killswitch", "_")
+
+        pose_topic = self.get_parameter("topics.pose").value
+        twist_topic = self.get_parameter("topics.twist").value
+        guidance_topic = self.get_parameter("topics.guidance.los").value
+        thrust_topic = self.get_parameter("topics.wrench_input").value
+        operation_topic = self.get_parameter("topics.operation_mode").value
+        killswitch_topic = self.get_parameter("topics.killswitch").value
 
         return (
             pose_topic,
             twist_topic,
             guidance_topic,
             thrust_topic,
-            softwareoperation_topic,
+            operation_topic,
             killswitch_topic,
         )
 
-    def declare_parameters(self) -> None:
+    def declare_params(self) -> None:
         """Declares parameters that are to be used from the configuration file."""
-        self.declare_parameter("LQR_params.q_surge")
-        self.declare_parameter("LQR_params.q_pitch")
-        self.declare_parameter("LQR_params.q_yaw")
+        self.get_logger().info("cool")
+
+        self.declare_parameter("LQR_params.q_surge", 0.0)
+        self.declare_parameter("LQR_params.q_pitch", 0.0)
+        self.declare_parameter("LQR_params.q_yaw", 0.0)
+
+        self.get_logger().info("yaw")
 
-        self.declare_parameter("LQR_params.r_surge")
-        self.declare_parameter("LQR_params.r_pitch")
-        self.declare_parameter("LQR_params.r_yaw")
+        self.declare_parameter("LQR_params.r_surge", 0.0)
+        self.declare_parameter("LQR_params.r_pitch", 0.0)
+        self.declare_parameter("LQR_params.r_yaw", 0.0)
 
-        self.declare_parameter("LQR_params.i_surge")
-        self.declare_parameter("LQR_params.i_pitch")
-        self.declare_parameter("LQR_params.i_yaw")
+        self.get_logger().info("shaw")
 
-        self.declare_parameter("LQR_params.i_weight")
+        self.declare_parameter("LQR_params.i_surge", 0.0)
+        self.declare_parameter("LQR_params.i_pitch", 0.0)
+        self.declare_parameter("LQR_params.i_yaw", 0.0)
 
-        self.declare_parameter("LQR_params.dt")
-        self.declare_parameter("max_force")
+        self.get_logger().info("garhamat")
 
-    def get_parameters(self) -> None:
+        self.declare_parameter("LQR_params.i_weight", 0.0)
+
+        self.declare_parameter("dt", 0.0)
+        self.declare_parameter("propulsion.thrusters.max", 0.0)
+
+    def get_params(self) -> None:
         """Gets the declared parameters from the configuration file."""
         self.lqr_params.q_surge = self.get_parameter("LQR_params.q_surge").value
         self.lqr_params.q_pitch = self.get_parameter("LQR_params.q_pitch").value
@@ -211,9 +218,9 @@ def get_parameters(self) -> None:
         self.lqr_params.i_yaw = self.get_parameter("LQR_params.i_yaw").value
 
         self.lqr_params.i_weight = self.get_parameter("LQR_params.i_weight").value
-        self.lqr_params.max_force = self.get_parameter("max_force").value
+        self.lqr_params.max_force = self.get_parameter("propulsion.thrusters.max").value
 
-        self.dt = self.get_parameter("LQR_params.dt").value
+        self.dt = self.get_parameter("dt").value
 
     def get_and_reshape_inertia_matrix(self) -> None:
         """Gets the inertia matrix from config and reshapes it to proper np array."""
@@ -291,14 +298,15 @@ def control_loop(self) -> None:
             self.controller.reset_controller()
             return
 
-        msg = Wrench()
+        msg = WrenchStamped()
 
         u = self.controller.calculate_lqr_u(
             self.coriolis_matrix, self.states, self.guidance_values
         )
-        msg.force.x = float(u[0])
-        msg.torque.y = float(u[1])
-        msg.torque.z = float(u[2])
+
+        msg.wrench.force.x = float(u[0])
+        msg.wrench.torque.y = float(u[1])
+        msg.wrench.torque.z = float(u[2])
 
         self.publisherLQR.publish(msg)