From c11a0721f99df99ed54417a1d6a87fa266ef97d4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sun, 26 Jan 2025 18:35:55 +0000 Subject: [PATCH] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../guidance_los/los_guidance_algorithm.py | 34 +++++++++---- .../scripts/los_guidance_action_server.py | 48 +++++++++++-------- 2 files changed, 54 insertions(+), 28 deletions(-) diff --git a/guidance/guidance_los/guidance_los/los_guidance_algorithm.py b/guidance/guidance_los/guidance_los/los_guidance_algorithm.py index ec4df98e1..a271bf58d 100644 --- a/guidance/guidance_los/guidance_los/los_guidance_algorithm.py +++ b/guidance/guidance_los/guidance_los/los_guidance_algorithm.py @@ -113,14 +113,22 @@ def compute_pitch_command( self.los_params.max_pitch_angle, ) - def compute_desired_speed(self, yaw_error: float, distance_to_target: float, crosstrack_y: float) -> float: - """ - Compute speed command with yaw error, distance and crosstrack-based scaling. + def compute_desired_speed( + self, yaw_error: float, distance_to_target: float, crosstrack_y: float + ) -> float: + """Compute speed command with yaw error, distance and crosstrack-based scaling. """ yaw_factor = np.cos(yaw_error) - distance_factor = min(1.0, distance_to_target / self.los_params.lookahead_distance_max) + distance_factor = min( + 1.0, distance_to_target / self.los_params.lookahead_distance_max + ) crosstrack_factor = 1.0 / (1.0 + abs(crosstrack_y)) - desired_speed = self.los_params.nominal_speed * yaw_factor * distance_factor * crosstrack_factor + desired_speed = ( + self.los_params.nominal_speed + * yaw_factor + * distance_factor + * crosstrack_factor + ) return max(self.los_params.min_speed, desired_speed) def apply_reference_filter(self, commands: State) -> State: @@ -217,15 +225,23 @@ def compute_theta_d( return theta_d def calculate_y_int_dot(self, crosstrack_y, y_int): - y_int_dot = (self.lookahead_h * crosstrack_y) / (self.lookahead_h**2 + (crosstrack_y + self.kappa * y_int)**2) + y_int_dot = (self.lookahead_h * crosstrack_y) / ( + self.lookahead_h**2 + (crosstrack_y + self.kappa * y_int) ** 2 + ) return y_int_dot - - def compute_psi_d(self, current_waypoint: State, next_waypoint: State, crosstrack_y: float, y_int: float) -> float: + + def compute_psi_d( + self, + current_waypoint: State, + next_waypoint: State, + crosstrack_y: float, + y_int: float, + ) -> float: pi_h = self.compute_pi_h(current_waypoint, next_waypoint) psi_d = pi_h - np.arctan((crosstrack_y + self.kappa) / self.lookahead_h) psi_d = self.ssa(psi_d) return psi_d - + @staticmethod def calculate_alpha_c(u: float, v: float, w: float, phi: float) -> float: if u == 0: diff --git a/guidance/guidance_los/scripts/los_guidance_action_server.py b/guidance/guidance_los/scripts/los_guidance_action_server.py index db8c653b8..d5ed73ebb 100755 --- a/guidance/guidance_los/scripts/los_guidance_action_server.py +++ b/guidance/guidance_los/scripts/los_guidance_action_server.py @@ -46,11 +46,11 @@ def __init__(self): self.declare_parameter('update_rate', 10.0) update_rate = self.get_parameter('update_rate').value self.update_period = 1.0 / update_rate - + self.waiting_at_waypoint = False self.wait_cycles = int(1.0 / self.update_period) self.wait_count = 0 - self.y_int = 0.0 + self.y_int = 0.0 # Initialize filter status flag self.filter_initialized = False @@ -221,40 +221,44 @@ def twist_callback(self, msg: TwistWithCovarianceStamped): self.state.twist = twist_from_ros(msg.twist.twist) def calculate_guidance(self) -> State: - error = self.guidance_calculator.state_as_pos_array(self.state - self.waypoints[self.current_waypoint_index]) + error = self.guidance_calculator.state_as_pos_array( + self.state - self.waypoints[self.current_waypoint_index] + ) _, crosstrack_y, crosstrack_z = self.rotation_yz @ error - + alpha_c = self.guidance_calculator.calculate_alpha_c( self.state.twist.linear_x, - self.state.twist.linear_y, + self.state.twist.linear_y, self.state.twist.linear_z, - self.state.pose.pitch + self.state.pose.pitch, ) - + beta_c = self.guidance_calculator.calculate_beta_c( self.state.twist.linear_x, self.state.twist.linear_y, self.state.twist.linear_z, self.state.pose.roll, self.state.pose.pitch, - alpha_c + 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, - self.y_int + self.y_int, + ) + + y_int_dot = self.guidance_calculator.calculate_y_int_dot( + crosstrack_y, self.y_int ) - - y_int_dot = self.guidance_calculator.calculate_y_int_dot(crosstrack_y, self.y_int) self.y_int += y_int_dot * self.update_period - + 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 + alpha_c, ) yaw_error = ssa(psi_d - self.state.pose.yaw) @@ -266,15 +270,17 @@ def calculate_guidance(self) -> State: unfiltered_commands.twist.linear_x = desired_surge unfiltered_commands.pose.pitch = theta_d unfiltered_commands.pose.yaw = psi_d - - filtered_commands = self.guidance_calculator.apply_reference_filter(unfiltered_commands) + + filtered_commands = self.guidance_calculator.apply_reference_filter( + unfiltered_commands + ) return filtered_commands def execute_callback(self, goal_handle: ServerGoalHandle): """Execute waypoint navigation action.""" with self._goal_lock: self.goal_handle = goal_handle - + self.current_waypoint_index = 0 self.waypoints = [self.state] for waypoint in goal_handle.request.waypoints: @@ -296,7 +302,9 @@ def execute_callback(self, goal_handle: ServerGoalHandle): self.rotation_yz = rotation_y.T @ rotation_z.T # Initialize integral control - initial_error = self.guidance_calculator.state_as_pos_array(self.state - self.waypoints[self.current_waypoint_index]) + initial_error = self.guidance_calculator.state_as_pos_array( + self.state - self.waypoints[self.current_waypoint_index] + ) _, initial_crosstrack_y, _ = self.rotation_yz @ initial_error self.y_int = -initial_crosstrack_y / self.guidance_calculator.kappa @@ -343,7 +351,9 @@ def execute_callback(self, goal_handle: ServerGoalHandle): else: self.current_waypoint_index += 1 # Reset integral term for new waypoint - initial_error = self.guidance_calculator.state_as_pos_array(self.state - self.waypoints[self.current_waypoint_index]) + initial_error = self.guidance_calculator.state_as_pos_array( + self.state - self.waypoints[self.current_waypoint_index] + ) _, initial_crosstrack_y, _ = self.rotation_yz @ initial_error self.y_int = -initial_crosstrack_y / self.guidance_calculator.kappa waiting_at_waypoint = False