436 task guidance node for autopilot

guidance/guidance_los/config/los_guidance_params.yaml

@@ -1,5 +1,4 @@
 los_guidance_node:
-  # Node configurations
   ros__parameters:
     los_guidance:
       h_delta_min: 1.0
@@ -13,7 +12,6 @@ los_guidance_node:
         omega_diag: [2.5, 2.5, 2.5]
         zeta_diag: [0.7, 0.7, 0.7]
 
-    # Topic configurations
     topics:
       publishers:
         los_commands: '/guidance/LOS_commands'
@@ -24,7 +22,6 @@ los_guidance_node:
       subscribers:
         odometry: '/nucleus/odom'
 
-    # QoS configurations
     qos:
       publisher_depth: 10
       subscriber_depth: 10

guidance/guidance_los/guidance_los/los_guidance_algorithm.py

@@ -1,28 +1,37 @@
-#!/usr/bin/env python3
-
 import numpy as np
 
 
 class ThirdOrderLOSGuidance:
-    """Line-of-Sight (LOS) guidance algorithm."""
+    """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):
-        # Initialize parameters
-        self.h_delta_min = config_dict['h_delta_min']
-        self.h_delta_max = config_dict['h_delta_max']
-        self.h_delta_factor = config_dict['h_delta_factor']
-        self.nominal_speed = config_dict['nominal_speed']
-        self.min_speed = config_dict['min_speed']
-        self.max_pitch_angle = config_dict['max_pitch_angle']
-        self.depth_gain = config_dict['depth_gain']
-
-        # Filter parameters
+        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(config_dict['filter']['omega_diag'])
-        self.zeta = np.diag(config_dict['filter']['zeta_diag'])
+        self.omega = np.diag(self.config_dict['filter']['omega_diag'])
+        self.zeta = np.diag(self.config_dict['filter']['zeta_diag'])
 
-        # Initialize filter matrices
-        self.setup_filter_matrices()
+    @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))
@@ -44,9 +53,6 @@ def setup_filter_matrices(self):
         # Input matrix
         self.Bd[6:9, :] = omega_cubed
 
-    def ssa(self, angle: float) -> float:
-        return (angle + np.pi) % (2 * np.pi) - np.pi
-
     def compute_raw_los_guidance(
         self, current_pos: np.ndarray, target_pos: np.ndarray
     ) -> tuple[np.ndarray, float, float]:

guidance/guidance_los/launch/guidance.launch.py

@@ -22,7 +22,7 @@ def generate_launch_description():
     config_path_arg = DeclareLaunchArgument(
         'config_path',
         default_value=config_file,
-        description='/home/badawi/Desktop/auto-pilot/src/vortex-auv/guidance/guidance_los/config/los_guidance_params.yaml',
+        description='Path to the LOS guidance config file',
     )
 
     # Create the node

guidance/guidance_los/scripts/los_guidance_action_server.py

@@ -1,11 +1,9 @@
 #!/usr/bin/env python3
 
-import os
 from dataclasses import dataclass
 
 import numpy as np
 import rclpy
-import yaml
 from geometry_msgs.msg import PoseStamped, Vector3Stamped
 from guidance_los.los_guidance_algorithm import ThirdOrderLOSGuidance
 from nav_msgs.msg import Odometry
@@ -35,42 +33,12 @@ def as_ndarray(self) -> np.ndarray:
         return np.array([self.x, self.y, self.z, self.yaw, self.pitch])
 
 
-class GuidanceActionServer(Node):
+class LOSActionServer(Node):
     """ROS2 Action Server node for waypoint navigation using LOS guidance."""
 
     def __init__(self):
         super().__init__('guidance_action_server')
 
-        # Get the configuration file path from parameters
-        config_path = self.declare_parameter('config_path', '').value
-
-        # Load configuration
-        if config_path and os.path.exists(config_path):
-            self.get_logger().info(f"Loading parameters from: {config_path}")
-            try:
-                with open(config_path) as file:
-                    config_params = yaml.safe_load(file)
-                    node_params = config_params.get('los_guidance_node', {}).get(
-                        'ros__parameters', {}
-                    )
-
-                    # Extract different parameter groups
-                    self.los_params = node_params.get('los_guidance', {})
-                    self.topic_params = node_params.get('topics', {})
-                    self.qos_params = node_params.get('qos', {})
-
-                    self.get_logger().info("Configuration loaded successfully")
-            except Exception as e:
-                self.get_logger().error(f"Error loading config file: {e}")
-                self.los_params = {}
-                self.topic_params = {}
-                self.qos_params = {}
-        else:
-            self.get_logger().warn("No config file path provided. Using defaults.")
-            self.los_params = {}
-            self.topic_params = {}
-            self.qos_params = {}
-
         # Initialize filter status flag
         self.filter_initialized = False
 
@@ -86,6 +54,8 @@ def __init__(self):
         self._declare_topic_parameters()
         self._initialize_publishers()
         self._initialize_subscribers()
+        self._initialize_state()
+        self._initialize_action_server()
 
         # Initialize guidance calculator with third-order filtering
         self.guidance_calculator = ThirdOrderLOSGuidance(self.los_params)
@@ -152,7 +122,16 @@ def _initialize_subscribers(self):
             callback_group=self.timer_cb_group,
         )
 
-        # Action server for handling navigation requests
+    def _initialize_state(self):
+        """Initialize state and navigation variables."""
+        self.state: State = State()
+        self.waypoints: list[np.ndarray] = []
+        self.current_waypoint_index: int = 0
+        self.goal_handle: ServerGoalHandle[NavigateWaypoints] | None = None
+        self.update_period = 0.1
+
+    def _initialize_action_server(self):
+        """Initialize the navigation action server."""
         self._action_server = ActionServer(
             self,
             NavigateWaypoints,
@@ -161,15 +140,6 @@ def _initialize_subscribers(self):
             callback_group=self.action_cb_group,
         )
 
-        # State variables initialization
-        self.state: State = State()
-        self.waypoints: list[np.ndarray] = []
-        self.current_waypoint_index: int = 0
-        self.goal_handle: ServerGoalHandle[NavigateWaypoints] | None = None
-        self.update_period = 0.1
-
-        self.get_logger().info('Guidance Action Server initialized')
-
     def publish_log(self, message: str):
         """Publish debug log message to ROS topic and node logger (debug mode only)."""
         if not self.debug_mode:
@@ -181,89 +151,81 @@ def publish_log(self, message: str):
 
     def odom_callback(self, msg: Odometry):
         """Process odometry updates and trigger guidance calculations."""
-        try:
-            # Extract orientation quaternion to Euler angles
-            orientation_q = msg.pose.pose.orientation
-            roll, pitch, yaw = quat2euler(
-                [orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z]
-            )
-
-            # Update vehicle state
-            self.state.x = msg.pose.pose.position.x
-            self.state.y = msg.pose.pose.position.y
-            self.state.z = msg.pose.pose.position.z
-            self.state.yaw = yaw
-            self.state.pitch = pitch
-
-            # Initialize filter on first callback
-            if not self.filter_initialized:
-                initial_commands = np.array([0.0, 0.0, yaw])
-                self.guidance_calculator.reset_filter_state(initial_commands)
-                self.filter_initialized = True
-
-            # Log current position for debugging
-            self.publish_log(
-                f"Position: x={self.state.x:.3f}, "
-                f"y={self.state.y:.3f}, "
-                f"z={self.state.z:.3f}"
-            )
-            self.publish_log(
-                f"Orientation: roll={roll:.3f}, pitch={pitch:.3f}, yaw={yaw:.3f}"
-            )
+        # Extract orientation quaternion to Euler angles
+        orientation_q = msg.pose.pose.orientation
+        roll, pitch, yaw = quat2euler(
+            [orientation_q.w, orientation_q.x, orientation_q.y, orientation_q.z]
+        )
 
-        except Exception as e:
-            self.get_logger().error(f'Error in odom_callback: {str(e)}')
+        # Update vehicle state
+        self.state.x = msg.pose.pose.position.x
+        self.state.y = msg.pose.pose.position.y
+        self.state.z = msg.pose.pose.position.z
+        self.state.yaw = yaw
+        self.state.pitch = pitch
+
+        # Initialize filter on first callback
+        if not self.filter_initialized:
+            initial_commands = np.array([0.0, 0.0, yaw])
+            self.guidance_calculator.reset_filter_state(initial_commands)
+            self.filter_initialized = True
+
+        # Log current position for debugging
+        self.publish_log(
+            f"Position: x={self.state.x:.3f}, "
+            f"y={self.state.y:.3f}, "
+            f"z={self.state.z:.3f}"
+        )
+        self.publish_log(
+            f"Orientation: roll={roll:.3f}, pitch={pitch:.3f}, yaw={yaw:.3f}"
+        )
 
     def process_guidance(self):
         """Process guidance calculations and publish control commands."""
-        try:
-            if self.current_waypoint_index >= len(self.waypoints):
-                return
-
-            target_point = self.waypoints[self.current_waypoint_index]
+        if self.current_waypoint_index >= len(self.waypoints):
+            return
 
-            # Compute guidance commands using current state
-            commands, distance, depth_error = self.guidance_calculator.compute_guidance(
-                self.state.as_ndarray(), target_point, self.update_period
-            )
+        target_point = self.waypoints[self.current_waypoint_index]
 
-            # Publish commands and monitoring data
-            self.publish_guidance(commands)
-            self.publish_reference(target_point)
-            self.publish_errors(target_point, depth_error)
+        # Compute guidance commands using current state
+        commands, distance, depth_error = self.guidance_calculator.compute_guidance(
+            self.state.as_ndarray(), target_point, self.update_period
+        )
 
-            # Log guidance status
-            self.publish_log(
-                f"Guidance Status: surge={commands[0]:.2f}, "
-                f"pitch={commands[1]:.2f}, yaw={commands[2]:.2f}"
-            )
-            self.publish_log(f"Distance to target: {distance:.2f}")
+        # Publish commands and monitoring data
+        self.publish_guidance(commands)
+        self.publish_reference(target_point)
+        self.publish_errors(target_point, depth_error)
 
-            # Check if waypoint is reached (0.5m threshold)
-            if distance < 0.5:
-                self.publish_log(f'Reached waypoint {self.current_waypoint_index}')
+        # Log guidance status
+        self.publish_log(
+            f"Guidance Status: surge={commands[0]:.2f}, "
+            f"pitch={commands[1]:.2f}, yaw={commands[2]:.2f}"
+        )
+        self.publish_log(f"Distance to target: {distance:.2f}")
 
-                # Reset filter state for next waypoint
-                initial_commands = np.array([0.0, 0.0, self.state.yaw])
-                self.guidance_calculator.reset_filter_state(initial_commands)
+        # Check if waypoint is reached (0.5m threshold)
+        if distance < 0.5:
+            self.publish_log(f'Reached waypoint {self.current_waypoint_index}')
 
-                self.current_waypoint_index += 1
+            # Reset filter state for next waypoint
+            initial_commands = np.array([0.0, 0.0, self.state.yaw])
+            self.guidance_calculator.reset_filter_state(initial_commands)
 
-                # Check if all waypoints are reached
-                if self.current_waypoint_index >= len(self.waypoints):
-                    if self.goal_handle and self.goal_handle.is_active:
-                        self.goal_handle.succeed()
-                        self.goal_handle = None
-                    return
+            self.current_waypoint_index += 1
 
-            # Publish progress feedback
-            if self.goal_handle and self.goal_handle.is_active:
-                feedback_msg = NavigateWaypoints.Feedback()
-                feedback_msg.current_waypoint_index = float(self.current_waypoint_index)
-                self.goal_handle.publish_feedback(feedback_msg)
+            # Check if all waypoints are reached
+            if self.current_waypoint_index >= len(self.waypoints):
+                if self.goal_handle and self.goal_handle.is_active:
+                    self.goal_handle.succeed()
+                    self.goal_handle = None
+                return
 
-        except Exception as e:
-            self.get_logger().error(f'Error in process_guidance: {str(e)}')
+        # Publish progress feedback
+        if self.goal_handle and self.goal_handle.is_active:
+            feedback_msg = NavigateWaypoints.Feedback()
+            feedback_msg.current_waypoint_index = float(self.current_waypoint_index)
+            self.goal_handle.publish_feedback(feedback_msg)
 
     def execute_callback(self, goal_handle: ServerGoalHandle[NavigateWaypoints]):
         """Execute waypoint navigation action."""
@@ -311,9 +273,9 @@ def execute_callback(self, goal_handle: ServerGoalHandle[NavigateWaypoints]):
     def publish_guidance(self, commands):
         """Publish guidance commands for vehicle control."""
         msg = LOSGuidance()
-        msg.surge = commands[0]
-        msg.pitch = commands[1]
-        msg.yaw = commands[2]
+        msg.surge = commands[0]  # surge: forward velocity command
+        msg.pitch = commands[1]  # pitch: pitch angle command
+        msg.yaw = commands[2]  # yaw: yaw angle command
         self.guidance_cmd_pub.publish(msg)
 
     def publish_reference(self, target):
@@ -324,9 +286,9 @@ def publish_reference(self, target):
         msg = PoseStamped()
         msg.header.frame_id = "odom"
         msg.header.stamp = self.get_clock().now().to_msg()
-        msg.pose.position.x = target[0]
-        msg.pose.position.y = target[1]
-        msg.pose.position.z = target[2]
+        msg.pose.position.x = target[0]  # x: x-coordinate of the target point
+        msg.pose.position.y = target[1]  # y: y-coordinate of the target point
+        msg.pose.position.z = target[2]  # z: z-coordinate of the target point
         self.guidance_ref_pub.publish(msg)
 
     def publish_errors(self, target, depth_error):
@@ -348,7 +310,7 @@ def main(args=None):
     rclpy.init(args=args)
 
     # Create and initialize node
-    action_server = GuidanceActionServer()
+    action_server = LOSActionServer()
 
     # Create and use multithreaded executor for concurrent processing
     executor = MultiThreadedExecutor()