testing changes

guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml

@@ -5,4 +5,4 @@
       path_generator_order: 1 # Differentiability order
       time_to_max_speed: 10.0 # Time to reach maximum speed
       dt: 0.1 # Time step
-      u_desired: 0.5 # Desired speed
+      u_desired: 0.01 # Desired speed

motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py

@@ -8,14 +8,15 @@ def __init__(self):
         self.init_system()
 
     def init_system(self) -> None:
-        self.K_1 = np.diag([10, 10, 10]) # First gain matrix
-        self.K_2 = np.diag([60, 60, 30]) # Second gain matrix
+        self.K_1 = np.diag([0.2, 0.2, 0.2]) # First gain matrix
+        self.K_2 = np.diag([1, 1, 1]) # Second gain matrix
         self.tau_max = np.array([41.0, 50.0, 55.0]) # Må tilpasses thrusterne
 
         ## Forenklet modell ## Bør også endres
         m = 50
         self.M = np.diag([m, m, m])
         self.D = np.diag([10, 10, 5])
+        self.tau_max = np.array([100, 100, 100])
 
     def control_law(self, state: Odometry, reference: HybridpathReference) -> np.ndarray:
         """
@@ -63,11 +64,11 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda
         tau = -self.K_2 @ z2 + self.D @ nu + self.M @ sigma1 + self.M @ ds_alpha1 * (v_s + w)
 
         # Add constraints to tau # This should be improved
-        # for i in range(len(tau)):
-        #     if tau[i] > self.tau_max[i]:
-        #         tau[i] = self.tau_max[i]
-        #     elif tau[i] < -self.tau_max[i]:
-        #         tau[i] = -self.tau_max[i]
+        for i in range(len(tau)):
+            if tau[i] > self.tau_max[i]:
+                tau[i] = self.tau_max[i]
+            elif tau[i] < -self.tau_max[i]:
+                tau[i] = -self.tau_max[i]
 
         return tau
 

motion/lqr_controller/lqr_controller/lqr_controller.py

@@ -1,5 +1,4 @@
 import numpy as np
-import matplotlib.pyplot as plt
 from scipy.linalg import solve_continuous_are
 from lqr_controller.asv_model import ASV
 

motion/lqr_controller/lqr_controller/lqr_controller_node.py

@@ -5,6 +5,11 @@
 from nav_msgs.msg import Odometry
 from transforms3d.euler import quat2euler
 from lqr_controller.lqr_controller import LQRController
+from time import sleep
+
+from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSHistoryPolicy, QoSReliabilityPolicy
+qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, 
+                         reliability=QoSReliabilityPolicy.BEST_EFFORT)
 
 class LQRControllerNode(Node):
     def __init__(self):
@@ -21,27 +26,41 @@ def __init__(self):
             ])
 
         self.wrench_publisher_ = self.create_publisher(Wrench, "thrust/wrench_input", 1)
-        self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odometry/ned", self.state_cb, 1)
-        self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1)
+        self.state_subscriber_ = self.create_subscription(Odometry, "/sensor/seapath/odom/ned", self.state_cb, qos_profile=qos_profile)
+        # self.guidance_subscriber_ = self.create_subscription(Odometry, "controller/lqr/reference", self.guidance_cb, 1)
 
         m = self.get_parameter('lqr_controller.mass').get_parameter_value().double_value
         D = self.get_parameter('lqr_controller.D').get_parameter_value().double_array_value
         Q = self.get_parameter('lqr_controller.Q').get_parameter_value().double_array_value
         R = self.get_parameter('lqr_controller.R').get_parameter_value().double_array_value
 
-        self.get_logger().info(f"Mass: {m}")
-        self.get_logger().info(f"D: {D}")
-        self.get_logger().info(f"Q: {Q}")
-        self.get_logger().info(f"R: {R}")
+        # self.get_logger().info(f"Mass: {m}")
+        # self.get_logger().info(f"D: {D}")
+        # self.get_logger().info(f"Q: {Q}")
+        # self.get_logger().info(f"R: {R}")
 
         self.lqr_controller = LQRController(m, D, Q, R)
 
         # Using x, y, yaw as reference (1x3)
         self.x_ref = [0, 0, 0]
         self.state = [0, 0, 0, 0, 0, 0]
 
+        self.enabled = False
+
+        controller_period = 0.1
+        self.controller_timer_ = self.create_timer(controller_period, self.controller_callback)
+
+        self.test_oneMeterAhead()
+
         self.get_logger().info("lqr_controller_node started")
 
+    def test_oneMeterAhead(self):
+        self.get_logger().info("Sleeping 3 secs to get odom state")
+        sleep(3)
+        self.x_ref = [self.state[0] + 1, self.state[1], self.state[2]]
+        self.enabled = True
+        self.get_logger().info("LQR enabled")
+
     def odometrymsg_to_state(self, msg):
         x = msg.pose.pose.position.x
         y = msg.pose.pose.position.y
@@ -60,18 +79,23 @@ def odometrymsg_to_state(self, msg):
         state = np.array([x, y, yaw, vx, vy, vyaw])
         return state
 
-    def guidance_cb(self, msg):
-        self.x_ref = self.odometrymsg_to_state(msg)[:3]
+    # def guidance_cb(self, msg):
+    #     self.x_ref = self.odometrymsg_to_state(msg)[:3]
 
-        #wrench = self.run_lqr_to_wrench()
-        #self.wrench_publisher_.publish(wrench)
+    #     #wrench = self.run_lqr_to_wrench()
+    #     #self.wrench_publisher_.publish(wrench)
 
     def state_cb(self, msg):
         self.state = self.odometrymsg_to_state(msg)
 
-        wrench = self.run_lqr_to_wrench()
-        self.wrench_publisher_.publish(wrench)
-
+    def controller_callback(self):
+        """
+        Callback function for the controller timer. This function calculates the control input and publishes the control input.
+        """
+        if self.enabled:
+            wrench = self.run_lqr_to_wrench()
+            self.wrench_publisher_.publish(wrench)
+
     def run_lqr_to_wrench(self):
         self.lqr_controller.linearize_model(self.state[2])
         u = self.lqr_controller.calculate_control_input(self.state, self.x_ref, self.lqr_controller.K_LQR, self.lqr_controller.K_r)