Add params for testing manager with thrust allocator, + spin()

vortexntnu · Nov 4, 2023 · 6f91d03 · 6f91d03
1 parent 0832c05
commit 6f91d03
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 3 deletions.
diff --git a/asv_setup/config/freya.yaml b/asv_setup/config/freya.yaml
@@ -1,6 +1,10 @@
 /parameter_manager_node:
   ros__parameters:
-    configuration_matrix: "[[ 0.70711,   0.70711,   0.70711,   0.70711], [ -0.70711,  0.70711,   -0.70711,  0.70711], [ 0.27738,   0.27738,  -0.27738,  -0.27738]]" #NED #Surge #Sway #Yaw 
+    configuration_matrix: [0.70711,   0.70711,   0.70711,   0.70711, -0.70711,  0.70711,   -0.70711,  0.70711, 0.27738,   0.27738,  -0.27738,  -0.27738] #NED #Surge #Sway #Yaw 
+    configuration_matrix_rows: 3
+    configuration_matrix_columns: 4
+    asv_thruster_min_thrust: -100
+    asv_thruster_min_thrust: 100
     asv_thruster_manager_input: "thrust/desired_forces"  # 3DOF thrust vector
     asv_thruster_manager_output: "thrust/thruster_forces"
     asv_thruster_manager_wrench: "thrust/wrench_input"

diff --git a/asv_setup/scripts/parameter_manager_node.py b/asv_setup/scripts/parameter_manager_node.py
@@ -14,6 +14,11 @@ def __init__(self):
 
         #Declaration of parameters
         self.declare_parameter('configuration_matrix',"")
+        self.declare_parameter('configuration_matrix_rows', 0)
+        self.declare_parameter('configuration_matrix_columns', 0)
+        self.declare_parameter('asv_thruster_min_thrust', 0)
+        self.declare_parameter('asv_thruster_min_thrust', 0)
+
         self.declare_parameter('asv_thruster_manager_output',"")
         self.declare_parameter('asv_thruster_manager_input',"")
         self.declare_parameter('asv_thruster_manager_wrench',"")
@@ -23,7 +28,7 @@ def __init__(self):
         self.asv_thruster_manager_input = self.get_parameter('asv_thruster_manager_input').value
         self.asv_thruster_manager_wrench = self.get_parameter('asv_thruster_manager_wrench').value
 
-        self.configuration_matrix = StringInto2DArray(self.configuration_matrix)
+        # self.configuration_matrix = StringInto2DArray(self.configuration_matrix)
 
 
 def StringInto2DArray(input_string):
@@ -37,10 +42,10 @@ def StringInto2DArray(input_string):
 def main():
     rclpy.init()
     parameter_manager_node = ParameterManagerNode()
+    rclpy.spin(parameter_manager_node)
 
     #test = parameter_manager_node.configuration_matrix
     #print(test)
-
     parameter_manager_node.destroy_node()
     rclpy.shutdown()