refactor guidance action server

guidance/guidance_los/guidance_los/los_guidance_algorithm.py

@@ -71,14 +71,6 @@ class FilterParameters:
     omega_diag: np.ndarray = field(default_factory=lambda: np.array([1.0, 1.0, 1.0]))
     zeta_diag: np.ndarray = field(default_factory=lambda: np.array([1.0, 1.0, 1.0]))
 
-    def get_omega_matrix(self) -> np.ndarray:
-        """Get omega diagonal matrix."""
-        return np.diag(self.omega_diag)
-
-    def get_zeta_matrix(self) -> np.ndarray:
-        """Get zeta diagonal matrix."""
-        return np.diag(self.zeta_diag)
-
 
 class ThirdOrderLOSGuidance:
     """This class implements the Line-of-Sight (LOS) guidance algorithm.
@@ -172,8 +164,6 @@ def compute_guidance(self, current_pos: State, target_pos: State) -> State:
         filtered_commands.yaw = self.ssa(filtered_commands.yaw)
         return filtered_commands
 
-    def reset_filter_state(self, current_commands: np.ndarray) -> None:
+    def reset_filter_state(self, current_state: State) -> 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)
+        self.x[0:3] = current_state.as_los_array()

guidance/guidance_los/scripts/los_guidance_action_server.py

@@ -84,7 +84,7 @@ def declare_los_parameters_(self):
         self.declare_parameter('los_guidance.filter.omega_diag', [2.5, 2.5, 2.5])
         self.declare_parameter('los_guidance.filter.zeta_diag', [0.7, 0.7, 0.7])
 
-    def get_los_parameters_(self) -> dict:
+    def get_los_parameters_(self) -> LOSParameters:
         los_params = LOSParameters()
         los_params.lookahead_distance_min = self.get_parameter(
             'los_guidance.h_delta_min'
@@ -105,7 +105,7 @@ def get_los_parameters_(self) -> dict:
         los_params.depth_gain = self.get_parameter('los_guidance.depth_gain').value
         return los_params
 
-    def get_filter_parameters_(self) -> dict:
+    def get_filter_parameters_(self) -> FilterParameters:
         filter_params = FilterParameters()
         filter_params.omega_diag = self.get_parameter(
             'los_guidance.filter.omega_diag'
@@ -226,7 +226,7 @@ def cancel_callback(self, goal_handle: ServerGoalHandle):
 
                 # Reset guidance state if needed
                 initial_commands = np.array([0.0, self.state.pitch, self.state.yaw])
-                self.guidance_calculator.reset_filter_state(initial_commands)
+                self.guidance_calculator.reset_filter_state(self.state)
 
                 self.publish_log("Navigation canceled and state reset")
 
@@ -347,6 +347,7 @@ def execute_callback(self, goal_handle: ServerGoalHandle):
 
             while rclpy.ok():
                 if not goal_handle.is_active:
+                    # reset filter here
                     return NavigateWaypoints.Result(success=False)
 
                 if goal_handle.is_cancel_requested: