From 6a131bc7331f33df0d7e6056e78c9ad2f257a3b1 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 7 Jan 2025 14:49:53 +0000 Subject: [PATCH] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../scripts/los_guidance_action_server.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/guidance/guidance_los/scripts/los_guidance_action_server.py b/guidance/guidance_los/scripts/los_guidance_action_server.py index 800f6c97..6346d769 100755 --- a/guidance/guidance_los/scripts/los_guidance_action_server.py +++ b/guidance/guidance_los/scripts/los_guidance_action_server.py @@ -1,8 +1,9 @@ #!/usr/bin/env python3 +import threading + import numpy as np import rclpy -import threading from geometry_msgs.msg import PoseStamped, Vector3Stamped from guidance_los.los_guidance_algorithm import ( FilterParameters, @@ -12,7 +13,7 @@ ) from nav_msgs.msg import Odometry from rclpy.action import ActionServer -from rclpy.action.server import ServerGoalHandle, GoalResponse, CancelResponse +from rclpy.action.server import CancelResponse, GoalResponse, ServerGoalHandle from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node @@ -34,11 +35,10 @@ class LOSActionServer(Node): def __init__(self): super().__init__('los_guidance_node') - + # Initialize goal handle lock self._goal_lock = threading.Lock() - # update rate parameter self.declare_parameter('update_rate', 10.0) update_rate = self.get_parameter('update_rate').value @@ -195,23 +195,22 @@ def _initialize_action_server(self): cancel_callback=self.cancel_callback, callback_group=self.action_cb_group, ) - - + def goal_callback(self, goal_request): """Handle new goal requests with preemption.""" self.get_logger().info("Received new goal request") - + # Validate waypoints exist if not goal_request.waypoints: self.get_logger().warn("No waypoints in request, rejecting") return GoalResponse.REJECT - + with self._goal_lock: # If there's an active goal, preempt it if self.goal_handle and self.goal_handle.is_active: self.get_logger().info("Preempting current goal") self.goal_handle.abort() - + return GoalResponse.ACCEPT def cancel_callback(self, goal_handle): @@ -359,7 +358,7 @@ def execute_callback(self, goal_handle: ServerGoalHandle): with self._goal_lock: if self.goal_handle == goal_handle: self.goal_handle = None - + def publish_guidance(self, commands: State): """Publish the commanded surge velocity, pitch angle, and yaw angle.""" msg = LOSGuidance()