From 62b1fd8909ac5d780a869b53951cebd95bb6b26c Mon Sep 17 00:00:00 2001 From: abubakaraliyubadawi Date: Tue, 7 Jan 2025 16:49:49 +0100 Subject: [PATCH] refactor guidance action server --- .../guidance_los/los_guidance_algorithm.py | 14 ++------------ .../scripts/los_guidance_action_server.py | 7 ++++--- 2 files changed, 6 insertions(+), 15 deletions(-) diff --git a/guidance/guidance_los/guidance_los/los_guidance_algorithm.py b/guidance/guidance_los/guidance_los/los_guidance_algorithm.py index 9adb99c1..4e839896 100644 --- a/guidance/guidance_los/guidance_los/los_guidance_algorithm.py +++ b/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() \ No newline at end of file diff --git a/guidance/guidance_los/scripts/los_guidance_action_server.py b/guidance/guidance_los/scripts/los_guidance_action_server.py index 2c2deabf..32f49425 100755 --- a/guidance/guidance_los/scripts/los_guidance_action_server.py +++ b/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: