Getting closer to a perfect LOS Guidance

Author: AbubakarAliyuBadawi

guidance/guidance_los/guidance_los/los_guidance_algorithm.py

@@ -32,7 +32,7 @@ def __sub__(self, other: "State") -> "State":
 
     def as_los_array(self):
         return np.array([self.surge_vel, self.pitch, self.yaw])
-    
+
     def as_pos_array(self):
         return np.array([self.x, self.y, self.z])
 
@@ -212,46 +212,71 @@ def compute_pi_h(current_waypoint: State, next_waypoint: State) -> float:
         dx = next_waypoint.x - current_waypoint.x
         dy = next_waypoint.y - current_waypoint.y
         return np.arctan2(dy, dx)
-    
+
     @staticmethod
     def compute_pi_v(current_waypoint: State, next_waypoint: State) -> float:
         dz = next_waypoint.z - current_waypoint.z
-        horizontal_distance = np.sqrt((next_waypoint.x - current_waypoint.x)**2 + (next_waypoint.y - current_waypoint.y)**2)
+        horizontal_distance = np.sqrt(
+            (next_waypoint.x - current_waypoint.x) ** 2
+            + (next_waypoint.y - current_waypoint.y) ** 2
+        )
         return np.arctan2(-dz, horizontal_distance)
-    
+
     @staticmethod
     def compute_rotation_y(pi_v: float) -> np.ndarray:
-        return np.array([
-            [np.cos(pi_v), 0, np.sin(pi_v)],
-            [0, 1, 0],
-            [-np.sin(pi_v), 0, np.cos(pi_v)]
-        ])
-    
+        return np.array(
+            [
+                [np.cos(pi_v), 0, np.sin(pi_v)],
+                [0, 1, 0],
+                [-np.sin(pi_v), 0, np.cos(pi_v)],
+            ]
+        )
+
     @staticmethod
     def compute_rotation_z(pi_h: float) -> np.ndarray:
-        return np.array([
-            [np.cos(pi_h), -np.sin(pi_h), 0],
-            [np.sin(pi_h), np.cos(pi_h), 0],
-            [0, 0, 1]
-        ])
-    
-    def compute_psi_d(self, current_waypoint: State, next_waypoint: State, crosstrack_y: float, beta_c: float) -> float:
+        return np.array(
+            [
+                [np.cos(pi_h), -np.sin(pi_h), 0],
+                [np.sin(pi_h), np.cos(pi_h), 0],
+                [0, 0, 1],
+            ]
+        )
+
+    def compute_psi_d(
+        self,
+        current_waypoint: State,
+        next_waypoint: State,
+        crosstrack_y: float,
+        beta_c: float,
+    ) -> float:
         pi_h = self.compute_pi_h(current_waypoint, next_waypoint)
         psi_d = pi_h - np.arctan(crosstrack_y / self.lookahead_h) - beta_c
         psi_d = self.ssa(psi_d)
         return psi_d
-    
-    def compute_theta_d(self, current_waypoint: State, next_waypoint: State, crosstrack_z: float, alpha_c: float) -> float:
+
+    def compute_theta_d(
+        self,
+        current_waypoint: State,
+        next_waypoint: State,
+        crosstrack_z: float,
+        alpha_c: float,
+    ) -> float:
         pi_v = self.compute_pi_v(current_waypoint, next_waypoint)
         theta_d = pi_v + np.arctan(crosstrack_z / self.lookahead_v) + alpha_c
         theta_d = self.ssa(theta_d)
         return theta_d
 
     @staticmethod
     def calculate_alpha_c(u: float, v: float, w: float, phi: float) -> float:
-        return np.arctan((v * np.sin(phi) + w * np.cos(phi)) / u) # Slide 104 in Fossen 2024
-    
+        return np.arctan(
+            (v * np.sin(phi) + w * np.cos(phi)) / u
+        )  # Slide 104 in Fossen 2024
+
     @staticmethod
-    def calculate_beta_c(u: float, v: float, w: float, phi: float, theta: float, alpha_c: float) -> float:
-        U_v = u * np.sqrt(1 + np.tan(alpha_c)**2)
-        return np.arctan((v * np.cos(phi) - w * np.sin(phi)) / (U_v * np.cos(theta - alpha_c))) # Slide 104 in Fossen 2024
+    def calculate_beta_c(
+        u: float, v: float, w: float, phi: float, theta: float, alpha_c: float
+    ) -> float:
+        U_v = u * np.sqrt(1 + np.tan(alpha_c) ** 2)
+        return np.arctan(
+            (v * np.cos(phi) - w * np.sin(phi)) / (U_v * np.cos(theta - alpha_c))
+        )  # Slide 104 in Fossen 2024

guidance/guidance_los/scripts/los_guidance_action_server.py

@@ -136,7 +136,6 @@ def _declare_topic_parameters(self):
 
     def _initialize_publishers(self):
         """Initialize all publishers."""
-
         los_commands_topic = self.get_parameter('topics.publishers.los_commands').value
         self.get_logger().info(f"Publishing LOS commands to: {los_commands_topic}")
         self.guidance_cmd_pub = self.create_publisher(
@@ -221,24 +220,39 @@ def odom_callback(self, msg: Odometry):
             self.filter_initialized = True
 
     def calculate_guidance(self) -> State:
-        error = self.state.as_pos_array() - self.waypoints[self.current_waypoint_index].as_pos_array()
+        error = (
+            self.state.as_pos_array()
+            - self.waypoints[self.current_waypoint_index].as_pos_array()
+        )
         _, crosstrack_y, crosstrack_z = self.rotation_yz @ error
-        alpha_c = self.guidance_calculator.calculate_alpha_c(self.u, self.v, self.w, self.phi)
-        beta_c = self.guidance_calculator.calculate_beta_c(self.u, self.v, self.w, self.phi, self.pitch, alpha_c)
-        psi_d = self.guidance_calculator.compute_psi_d(self.waypoints[self.current_waypoint_index], 
-                                                       self.waypoints[self.current_waypoint_index + 1], 
-                                                       crosstrack_y, beta_c)
-        theta_d = self.guidance_calculator.compute_theta_d(self.waypoints[self.current_waypoint_index], 
-                                                           self.waypoints[self.current_waypoint_index + 1], 
-                                                           crosstrack_z, alpha_c)
-        unfiltered_commands = State(surge_vel=self.desired_vel, pitch=theta_d, yaw=psi_d)
-        filtered_commands = self.guidance_calculator.apply_reference_filter(unfiltered_commands)
+        alpha_c = self.guidance_calculator.calculate_alpha_c(
+            self.u, self.v, self.w, self.phi
+        )
+        beta_c = self.guidance_calculator.calculate_beta_c(
+            self.u, self.v, self.w, self.phi, self.pitch, alpha_c
+        )
+        psi_d = self.guidance_calculator.compute_psi_d(
+            self.waypoints[self.current_waypoint_index],
+            self.waypoints[self.current_waypoint_index + 1],
+            crosstrack_y,
+            beta_c,
+        )
+        theta_d = self.guidance_calculator.compute_theta_d(
+            self.waypoints[self.current_waypoint_index],
+            self.waypoints[self.current_waypoint_index + 1],
+            crosstrack_z,
+            alpha_c,
+        )
+        unfiltered_commands = State(
+            surge_vel=self.desired_vel, pitch=theta_d, yaw=psi_d
+        )
+        filtered_commands = self.guidance_calculator.apply_reference_filter(
+            unfiltered_commands
+        )
         return unfiltered_commands
 
-
     def execute_callback(self, goal_handle: ServerGoalHandle):
         """Execute waypoint navigation action."""
-
         # Initialize navigation goal with lock
         with self._goal_lock:
             self.goal_handle = goal_handle
@@ -253,8 +267,14 @@ def execute_callback(self, goal_handle: ServerGoalHandle):
             )
             self.waypoints.append(point_as_state)
 
-        self.pi_h = self.guidance_calculator.compute_pi_h(self.waypoints[self.current_waypoint_index], self.waypoints[self.current_waypoint_index + 1])
-        self.pi_v = self.guidance_calculator.compute_pi_v(self.waypoints[self.current_waypoint_index], self.waypoints[self.current_waypoint_index + 1])
+        self.pi_h = self.guidance_calculator.compute_pi_h(
+            self.waypoints[self.current_waypoint_index],
+            self.waypoints[self.current_waypoint_index + 1],
+        )
+        self.pi_v = self.guidance_calculator.compute_pi_v(
+            self.waypoints[self.current_waypoint_index],
+            self.waypoints[self.current_waypoint_index + 1],
+        )
 
         rotation_y = self.guidance_calculator.compute_rotation_y(self.pi_v)
         rotation_z = self.guidance_calculator.compute_rotation_z(self.pi_h)
@@ -280,22 +300,34 @@ def execute_callback(self, goal_handle: ServerGoalHandle):
                 self.goal_handle = None
                 result.success = False
                 return result
-            
-            norm = np.linalg.norm(self.state.as_pos_array() - self.waypoints[self.current_waypoint_index + 1].as_pos_array(), 2)
+
+            norm = np.linalg.norm(
+                self.state.as_pos_array()
+                - self.waypoints[self.current_waypoint_index + 1].as_pos_array(),
+                2,
+            )
             if norm < 0.5:
                 self.get_logger().info('Waypoint reached')
                 self.current_waypoint_index += 1
 
                 if self.current_waypoint_index >= len(self.waypoints) - 1:
                     self.get_logger().info('All waypoints reached!')
-                    final_commands = State(surge_vel=0.0, pitch=self.state.pitch, yaw=self.state.yaw)
+                    final_commands = State(
+                        surge_vel=0.0, pitch=self.state.pitch, yaw=self.state.yaw
+                    )
                     self.publish_guidance(final_commands)
                     result.success = True
                     self.goal_handle.succeed()
                     return result
-                
-                self.pi_h = self.guidance_calculator.compute_pi_h(self.waypoints[self.current_waypoint_index], self.waypoints[self.current_waypoint_index + 1])
-                self.pi_v = self.guidance_calculator.compute_pi_v(self.waypoints[self.current_waypoint_index], self.waypoints[self.current_waypoint_index + 1])
+
+                self.pi_h = self.guidance_calculator.compute_pi_h(
+                    self.waypoints[self.current_waypoint_index],
+                    self.waypoints[self.current_waypoint_index + 1],
+                )
+                self.pi_v = self.guidance_calculator.compute_pi_v(
+                    self.waypoints[self.current_waypoint_index],
+                    self.waypoints[self.current_waypoint_index + 1],
+                )
 
                 rotation_y = self.guidance_calculator.compute_rotation_y(self.pi_v)
                 rotation_z = self.guidance_calculator.compute_rotation_z(self.pi_h)
@@ -314,6 +346,7 @@ def publish_guidance(self, commands: State):
         msg.yaw = commands.yaw
         self.guidance_cmd_pub.publish(msg)
 
+
 def main(args=None):
     """Main function to initialize and run the guidance node."""
     rclpy.init(args=args)
@@ -332,4 +365,4 @@ def main(args=None):
 
 
 if __name__ == '__main__':
-    main()
+    main()