[pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci

Author: pre-commit-ci[bot]

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:

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