Seemingly working hybridpath_controller

asv_setup/launch/hybridpath.launch.py

@@ -0,0 +1,25 @@
+import os
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.actions import SetEnvironmentVariable, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    hybridpath_controller_launch = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('hybridpath_controller'), 'launch/hybridpath_controller.launch.py')
+        )
+    )
+
+    hybridpath_guidance_launch = IncludeLaunchDescription(
+    PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('hybridpath_guidance'), 'launch/hybridpath_guidance.launch.py')
+        )
+    )
+
+    # Return launch description
+    return LaunchDescription([
+        hybridpath_controller_launch,
+        hybridpath_guidance_launch
+    ])

motion/hybridpath_controller/config/hybridpath_controller_config.yaml

@@ -1,3 +1,4 @@
 /**:
   ros__parameters:
     hybridpath_controller:
+      kappa: 0.5

motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py

@@ -3,15 +3,14 @@
 from hybridpath_controller.hybridpath import HybridPathGenerator, HybridPathSignals
 
 class AdaptiveBackstep:
-    def __init__(self):
-        self.init_system()
+    def __init__(self, kappa):
+        self.init_system(kappa)
 
-    def init_system(self):
+    def init_system(self, kappa):
 
         I = np.eye(3)
 
         K_1 = np.diag([10, 10, 10])
-        kappa = 0.5
         self.K_1_tilde = K_1 + kappa*I
         self.K_2 = np.diag([60, 60, 30])
         self.tau_max = np.array([41.0, 50.0, 55.0]) # Må tilpasses thrusterne
@@ -56,9 +55,6 @@ def control_law(self, eta, nu, w, v_ref, dt_v_ref, dtheta_v_ref, eta_d, dtheta_e
                 tau[0] = np.sign(tau[0]) * np.absolute(self.tau_max[2] / tau[2]) * np.absolute(tau[0])
                 tau[2] = np.sign(tau[2]) * self.tau_max[2]
         return tau
-
-    def step(self, eta, nu, tau):
-        pass
 
     def calculate_coriolis_matrix(self, nu):
         # u = nu[0]