Fixes Finned Vehicle Modules

uuv_assistants/templates/robot_model/urdf/snippets.xacro.template

@@ -201,15 +201,15 @@
   <xacro:property name="fin_max_joint_limit" value="${0.0 * d2r}"/>
 
   <xacro:macro name="fin_macro" params="namespace fin_id *origin">
-      <joint name="${namespace}/fin${fin_id}_joint" type="revolute">
+      <joint name="${namespace}/fin_${fin_id}_joint" type="revolute">
           <limit effort="0" lower="${fin_min_joint_limit}" upper="${fin_max_joint_limit}" velocity="0"/>
           <xacro:insert_block name="origin"/>
           <axis xyz="0 0 1"/>
           <parent link="${namespace}/base_link" />
-          <child link="${namespace}/fin${fin_id}" />
+          <child link="${namespace}/fin_${fin_id}" />
       </joint>
 
-      <link name="${namespace}/fin${fin_id}">
+      <link name="${namespace}/fin_${fin_id}">
         <inertial>
           <mass value="0.001" />
           <origin xyz="0 0 0" rpy="0 0 0"/>
@@ -279,8 +279,9 @@
           <current_velocity_topic>/hydrodynamics/current_velocity</current_velocity_topic>
 
           <!-- Name of the correspodent fin link and joint  -->
-          <link_name>${namespace}/fin${fin_id}</link_name>
-          <joint_name>${namespace}/fin${fin_id}_joint</joint_name>
+          <link_name>${namespace}/fin_${fin_id}</link_name>
+          <joint_name>${namespace}/fin_${fin_id}_joint</joint_name>
+          <fin_id>${fin_id}</fin_id>
 
           <!-- Output topic to publish the current angle of the fin joint -->
           <output_topic>${namespace}/fins/${fin_id}/output</output_topic>

uuv_control/uuv_auv_control_allocator/config/actuator_manager.yaml

@@ -0,0 +1,64 @@
+# $ROBOT_NAME:
+  auv_control_allocator:
+    ros__parameters:
+      base_link: base_link
+      update_rate: 50
+      timeout: -1
+
+      thruster_config:
+        topic_prefix: thrusters/
+        topic_suffix: /input
+        frame_base: thruster_
+        max_thrust: <set the maximum absolute thrust force allowed>        
+
+        ##################################################
+        # Options to set the thruster models below
+        ##################################################
+
+        # 1) If all thrusters have the same model (as described in the vehicle's robot description)
+        #
+        # 1.1) If the conversion function set for the thruster plugins is the following:
+        # <conversion>
+        #   <type>Basic</type>
+        #   <rotorConstant>0.0</rotorConstant>
+        # </conversion>
+        # You can set the conversion function to be:
+        conversion_fcn: proportional
+        conversion_fcn_params:
+          gain: <rotorConstant>
+
+        # 1.2)  If the conversion function set for the thruster plugins is the following:
+        # <conversion>
+        #   <type>LinearInterp</type>
+        #   <inputValues>0 1 2 3</inputValues>
+        #   <outputValues>0 1 2 3</outputValues>
+        # </conversion>
+        # You can set the conversion function to be:
+        conversion_fcn: custom
+        conversion_fcn_params:
+          input: [0, 1, 2, 3]
+          output: [0, 1, 2, 3]
+
+        # 2) If the vehicle has thrusters with different models, you can list the
+        #    models descriptions. Beware to set the list where the index in list
+        #    matches the thruster ID
+        conversion_fcn:
+          - proportional
+          - proportional
+          - custom
+        conversion_fcn_params:
+          - gain: rotorConstant0
+          - gain: rotorConstant1
+          - input: [0, 1, 2, 3]
+            output: [0, 1, 2, 3]
+
+      fin_config:
+        # Fin Configuration
+        topic_prefix: fins/
+        topic_suffix: /input
+        frame_base: fin_
+        lower_limit: 0.0 # Radians
+        upper_limit: 0.0
+        fin_area: 0.0
+        fluid_density: 1028.0
+        lift_coefficient: 0.0

uuv_control/uuv_auv_control_allocator/launch/start_control_allocator.launch.py

@@ -0,0 +1,50 @@
+import os
+import sys
+
+from launch import LaunchDescription
+from launch.actions.declare_launch_argument import DeclareLaunchArgument
+from launch_ros.actions import Node, PushRosNamespace
+from launch.actions.opaque_function import OpaqueFunction
+from launch.substitutions import LaunchConfiguration as LC
+from launch.actions import GroupAction
+
+import launch
+import launch.actions
+
+from ament_index_python.packages import get_package_share_directory
+
+bool_aliases = [1, '1', "True", "true"]
+
+def launch_setup(context, *args, **kwargs):
+    tam_reset_arg = {"output_dir": LC("output_dir").perform(context)} if (LC("reset_tam").perform(context) in bool_aliases) else LC("tam_config").perform(context)
+    fin_manager_node = Node(
+        name="auv_control_allocator",
+        package="uuv_auv_control_allocator",
+        executable="control_allocator",
+        output="screen",
+        parameters=[LC("config_file").perform(context)],
+        remappings=[("tf", "/tf")])
+
+    group = GroupAction([
+        PushRosNamespace(LC("namespace").perform(context)), # replace with vehicle namespace
+        fin_manager_node
+        ])
+
+    return [group]
+
+def generate_launch_description():
+
+    ld = launch.LaunchDescription([
+        DeclareLaunchArgument("namespace", default_value="ROBOT_NAME"), # Add your robot name here
+        DeclareLaunchArgument("reset_tam", default_value="True"), # TODO: Fix TAM Matrix Storage and Loading, need TAM?
+        DeclareLaunchArgument("config_file", default_value=os.path.join(get_package_share_directory("uuv_auv_control_allocator"), "config", "actuator_manager.yaml")),
+        #DeclareLaunchArgument("tam_config", default_value=os.path.join(get_package_share_directory("uuv_auv_control_allocator"), "config", "TAM.yaml")),
+        DeclareLaunchArgument("output_dir", default_value=os.path.join(get_package_share_directory("uuv_auv_control_allocator"), "config")),
+        OpaqueFunction(function=launch_setup)
+    ])
+
+    return ld
+
+
+if __name__ == '__main__':
+    generate_launch_description()

uuv_control/uuv_auv_control_allocator/scripts/control_allocator

@@ -33,16 +33,9 @@ from uuv_auv_actuator_interface import ActuatorManager
 from plankton_utils.time import time_in_float_sec
 from plankton_utils.time import is_sim_time
 
-
-# TODO Probably refactor
 class AUVControlAllocator(ActuatorManager):
-    def __init__(self, node_name, **kwargs):
-        #super(AUVControlAllocator, self).__init__()
-        ActuatorManager.__init__(
-            node_name, 
-            **kwargs
-        )
-
+    def __init__(self, name, **kwargs):
+        ActuatorManager.__init__(self, name, **kwargs) 
         # Retrieve the output file path to store the CAM
         # matrix for future use
         self.output_dir = None
@@ -53,11 +46,12 @@ class AUVControlAllocator(ActuatorManager):
                     'Invalid output directory, output_dir=' + self.output_dir)
             self.get_logger().info('output_dir=' + str(self.output_dir))
 
-        self.update_rate = self.get_parameter('update_rate', 10).value
+        self.update_rate = self.get_parameter('update_rate').value
+
         if self.update_rate <= 0:
             raise RuntimeError('Update rate must be a positive number')
 
-        self.timeout = self.get_parameter('timeout', -1).value       
+        self.timeout = self.get_parameter('timeout').value       
 
         # Control allocation matrix for thruster and fin lift forces
         self.u = casadi.SX.sym('u')
@@ -69,29 +63,40 @@ class AUVControlAllocator(ActuatorManager):
         self.input_surge_speed = 0.0
         self.input_tau = np.zeros(6)        
 
-        if not self.init():
+        self.init_future.add_done_callback(self._init_class)
+    # =========================================================================
+    def _init_class(self, future):
+        """ 
+        Initializes the class. Called as a callback when the parent class 
+        is initialized
+        """
+        try:
+            self._init_class_impl()
+        except Exception as e:
+            self.get_logger().info('Caught exception: ' + repr(e))
+    # =========================================================================
+    def _init_class_impl(self):
+        if not self.update_cam():
             raise RuntimeError('No thruster and/or fins found')
 
-        self.sub = self.create_subscription(AUVCommand, 'control_input', self.control_callback, 10)
+        self.sub = self.create_subscription(AUVCommand, "/" + self.namespace + '/control_input', self.control_callback, 10)
 
         # NB : no spin before, the sim time clock() is automatically 0
         self.last_update = time_in_float_sec(self.get_clock().now())
 
-        rate = self.create_rate(self.update_rate)
-        thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
-        thread.start()
-        while rclpy.ok():
-            #rclpy.spin_once(self)
-            if self.timeout > -1:
-                if time_in_float_sec(self.get_clock().now()) - self.last_update > self.timeout:
-                    self.input_surge_speed = 0.0
-                    self.input_tau = np.zeros(6)            
-            output = self.allocate(self.input_tau, self.input_surge_speed)            
-            self.publish_commands(output)
-            rate.sleep()
+        self.timer_function = self.create_timer(1.0 / self.update_rate, self.spin_function)
 
+        self.get_logger().info('ControlAllocator: ready')
     # =========================================================================
-    def init(self):
+    def spin_function(self):
+        if self.timeout > -1:
+            if time_in_float_sec(self.get_clock().now()) - self.last_update > self.timeout:
+                self.input_surge_speed = 0.0
+                self.input_tau = np.zeros(6)          
+        output = self.allocate(self.input_tau, self.input_surge_speed)          
+        self.publish_commands(output)
+    # =========================================================================
+    def update_cam(self):
         """Calculate the control allocation matrix, if one is not given."""
 
         # Build the casadi model
@@ -155,7 +160,7 @@ class AUVControlAllocator(ActuatorManager):
             self.input_tau[0:3] = np.dot(self.base_link_ned_to_enu, self.input_tau[0:3])
             self.input_tau[4::] = np.dot(self.base_link_ned_to_enu, self.input_tau[4::])
 
-        self.last_update = self.get_clock().now()
+        self.last_update = time_in_float_sec(self.get_clock().now())
 
 
 # =============================================================================
@@ -166,15 +171,14 @@ def main():
         sim_time_param = is_sim_time()
 
         node = AUVControlAllocator('auv_control_allocator', parameter_overrides=[sim_time_param])
-
-    except rclpy.ROSInterruptException:
-        print('AUVControlAllocator::ROSInterruptException Exception')
+        rclpy.spin(node)
+
     except Exception as e:
-        print('Caught exception: ' + str(e))
+        print('AUVControlAllocatoreNode::Exception: ' + str(e))
     finally:
         if rclpy.ok():
             rclpy.shutdown()
-    print('Leaving AUVControlAllocator')
+    print('Leaving AUVControlAllocatorNode')
 
 
 # =============================================================================