Skip to content

Commit

Permalink
[pre-commit.ci] auto fixes from pre-commit.com hooks
Browse files Browse the repository at this point in the history
for more information, see https://pre-commit.ci
  • Loading branch information
pre-commit-ci[bot] committed Jan 7, 2025
1 parent 3160f92 commit 6a131bc
Showing 1 changed file with 9 additions and 10 deletions.
19 changes: 9 additions & 10 deletions guidance/guidance_los/scripts/los_guidance_action_server.py
Original file line number Diff line number Diff line change
@@ -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,
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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()
Expand Down

0 comments on commit 6a131bc

Please sign in to comment.