436 task guidance node for autopilot

.github/workflows/source-build.yaml

@@ -12,5 +12,6 @@ jobs:
     with:
       ros_distro: 'humble'
       os_name: 'ubuntu-22.04'
-      ref: ${{ github.ref_name }}
+      ref: ${{ github.ref }}
       vcs_repo_file_url: 'https://raw.githubusercontent.com/vortexntnu/vortex-auv/main/ros2.repos'
+      dependency_script: 'requirements.sh'

.pre-commit-config.yaml

@@ -56,7 +56,6 @@ repos:
             "--select=D",
             "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401",
             "--fix",
-            "--unsafe-fixes"
             ]
         stages: [pre-commit]
         pass_filenames: true

guidance/guidance_los/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.8)
+project(guidance_los)
+
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(
+  PROGRAMS
+    scripts/los_guidance_action_server.py
+  RENAME
+    los_guidance_action_server
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

guidance/guidance_los/README.md

@@ -0,0 +1 @@
+#TODO

guidance/guidance_los/config/los_guidance_params.yaml

@@ -0,0 +1,27 @@
+los_guidance_node:
+  ros__parameters:
+    los_guidance:
+      h_delta_min: 1.0
+      h_delta_max: 5.0
+      h_delta_factor: 3.0
+      nominal_speed: 0.35
+      min_speed: 0.1
+      max_pitch_angle: 0.52359877559
+      depth_gain: 0.5
+      filter:
+        omega_diag: [2.5, 2.5, 2.5]
+        zeta_diag: [0.7, 0.7, 0.7]
+
+    topics:
+      publishers:
+        los_commands: '/guidance/LOS_commands'
+        debug:
+          reference: '/guidance/debug/reference'
+          errors: '/guidance/debug/errors'
+          logs: '/guidance/debug/logs'
+      subscribers:
+        odometry: '/nucleus/odom'
+
+    qos:
+      publisher_depth: 10
+      subscriber_depth: 10

guidance/guidance_los/guidance_los/los_guidance_algorithm.py

@@ -0,0 +1,120 @@
+import numpy as np
+
+
+class ThirdOrderLOSGuidance:
+    """This class implements the Line-of-Sight (LOS) guidance algorithm.
+
+    The LOS provide a control outputs for surge, pitch, and yaw for navigating towards a target in 3D space.
+    """
+
+    def __init__(self, config_dict: dict):
+        self.config_dict = config_dict
+        self.init_los_parameters()
+        self.init_filter_parameters()
+        self.setup_filter_matrices()
+
+    def init_los_parameters(self):
+        """Initialize Line-of-Sight guidance parameters."""
+        self.h_delta_min = self.config_dict['h_delta_min']
+        self.h_delta_max = self.config_dict['h_delta_max']
+        self.h_delta_factor = self.config_dict['h_delta_factor']
+        self.nominal_speed = self.config_dict['nominal_speed']
+        self.min_speed = self.config_dict['min_speed']
+        self.max_pitch_angle = self.config_dict['max_pitch_angle']
+        self.depth_gain = self.config_dict['depth_gain']
+
+    def init_filter_parameters(self):
+        """Initialize filter state and parameters."""
+        self.x = np.zeros(9)
+        self.omega = np.diag(self.config_dict['filter']['omega_diag'])
+        self.zeta = np.diag(self.config_dict['filter']['zeta_diag'])
+
+    @staticmethod
+    def ssa(angle: float) -> float:
+        return (angle + np.pi) % (2 * np.pi) - np.pi
+
+    def setup_filter_matrices(self):
+        self.Ad = np.zeros((9, 9))
+        self.Bd = np.zeros((9, 3))
+
+        # Fill Ad matrix blocks
+        self.Ad[0:3, 3:6] = np.eye(3)
+        self.Ad[3:6, 6:9] = np.eye(3)
+
+        # Compute characteristic polynomial coefficients
+        omega_cubed = self.omega @ self.omega @ self.omega
+        omega_squared = self.omega @ self.omega
+
+        # Fill in the acceleration dynamics
+        self.Ad[6:9, 0:3] = -omega_cubed
+        self.Ad[6:9, 3:6] = -(2 * self.zeta + np.eye(3)) @ omega_squared
+        self.Ad[6:9, 6:9] = -(2 * self.zeta + np.eye(3)) @ self.omega
+
+        # Input matrix
+        self.Bd[6:9, :] = omega_cubed
+
+    def compute_raw_los_guidance(
+        self, current_pos: np.ndarray, target_pos: np.ndarray
+    ) -> tuple[np.ndarray, float, float]:
+        # Extract positions
+        dx = target_pos[0] - current_pos[0]
+        dy = target_pos[1] - current_pos[1]
+
+        # Horizontal plane calculations
+        horizontal_distance = np.sqrt(dx**2 + dy**2)
+        desired_yaw = np.arctan2(dy, dx)
+        yaw_error = self.ssa(desired_yaw - current_pos[3])
+
+        # Vertical plane calculations
+        depth_error = target_pos[2] - current_pos[2]
+        desired_pitch = self.compute_pitch_command(depth_error, horizontal_distance)
+
+        # Compute desired surge velocity
+        desired_surge = self.compute_desired_speed(yaw_error, horizontal_distance)
+
+        # Return commands as numpy array
+        commands = np.array([desired_surge, desired_pitch, desired_yaw])
+        return commands, horizontal_distance, depth_error
+
+    def compute_pitch_command(
+        self, depth_error: float, horizontal_distance: float
+    ) -> float:
+        """Compute pitch command with distance-based scaling."""
+        raw_pitch = -self.depth_gain * depth_error
+        distance_factor = min(1.0, horizontal_distance / self.h_delta_max)
+        modified_pitch = raw_pitch * distance_factor
+        return np.clip(modified_pitch, -self.max_pitch_angle, self.max_pitch_angle)
+
+    def compute_desired_speed(self, yaw_error: float, distance: float) -> float:
+        """Compute speed command with yaw and distance-based scaling."""
+        yaw_factor = np.cos(yaw_error)
+        distance_factor = min(1.0, distance / self.h_delta_max)
+        desired_speed = self.nominal_speed * yaw_factor * distance_factor
+        return max(self.min_speed, desired_speed)
+
+    def apply_reference_filter(self, commands: np.ndarray, dt: float) -> np.ndarray:
+        # Update filter state
+        x_dot = self.Ad @ self.x + self.Bd @ commands
+        self.x = self.x + x_dot * dt
+        return self.x[0:3]  # Return position states [surge, pitch, yaw]
+
+    def compute_guidance(
+        self, current_pos: np.ndarray, target_pos: np.ndarray, dt: float
+    ) -> np.ndarray:
+        # Compute raw LOS guidance
+        (
+            raw_commands,
+            self.horizontal_distance_to_target,
+            self.vertical_distance_to_target,
+        ) = self.compute_raw_los_guidance(current_pos, target_pos)
+
+        # Apply reference filter
+        filtered_commands = self.apply_reference_filter(raw_commands, dt)
+        filtered_commands[2] = self.ssa(filtered_commands[2])
+        return filtered_commands
+
+    def reset_filter_state(self, current_commands: np.ndarray) -> None:
+        self.x = np.zeros(9)
+        self.x[0:3] = current_commands
+        self.x[3:6] = np.zeros(3)
+        self.x[6:9] = np.zeros(3)

guidance/guidance_los/launch/guidance.launch.py

@@ -0,0 +1,42 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Get the package share directory
+    pkg_share = get_package_share_directory('guidance_los')
+
+    # Build the config file path
+    config_file = os.path.join(pkg_share, 'config', 'los_guidance_params.yaml')
+
+    # Verify config file exists
+    if not os.path.exists(config_file):
+        raise FileNotFoundError(f"Config file not found at: {config_file}")
+
+    # Create the launch configuration variables
+    config_path_arg = DeclareLaunchArgument(
+        'config_path',
+        default_value=config_file,
+        description='Path to the LOS guidance config file',
+    )
+
+    # Create the node
+    guidance_node = Node(
+        package='guidance_los',
+        executable='los_guidance_action_server',
+        name='guidance_action_server',
+        output='screen',
+        emulate_tty=True,
+        parameters=[
+            {'config_path': LaunchConfiguration('config_path'), 'debug_mode': False}
+        ],
+        respawn=False,
+    )
+
+    # Return the launch description
+    return LaunchDescription([config_path_arg, guidance_node])

guidance/guidance_los/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>guidance_los</name>
+  <version>0.0.0</version>
+  <description>Package for implementing a LOS guidance action server.</description>
+  <maintainer email="[email protected]">badawi</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>tf_transformations</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>