68 lines
2.6 KiB
Python
68 lines
2.6 KiB
Python
import rclpy
|
|
from rclpy.action import ActionClient
|
|
from rclpy.node import Node
|
|
import json
|
|
from typing import Dict, Any
|
|
import logging
|
|
|
|
from drone_interfaces.action import ExecuteMission
|
|
from .websocket_manager import websocket_manager
|
|
|
|
class MissionActionClient(Node):
|
|
"""
|
|
Interfaces with the drone's `ExecuteMission` ROS2 Action Server.
|
|
"""
|
|
def __init__(self):
|
|
super().__init__('mission_action_client')
|
|
self._action_client = ActionClient(self, ExecuteMission, 'execute_mission')
|
|
self.get_logger().info("MissionActionClient initialized.")
|
|
|
|
def send_goal(self, py_tree: Dict[str, Any]):
|
|
"""
|
|
Sends the mission (py_tree) to the action server.
|
|
"""
|
|
if not self._action_client.server_is_ready():
|
|
self.get_logger().error("Action server not available, goal not sent.")
|
|
# Optionally, you could broadcast a status update to the frontend here
|
|
return
|
|
|
|
self.get_logger().info("Received request to send goal to drone.")
|
|
goal_msg = ExecuteMission.Goal()
|
|
goal_msg.py_tree_json = json.dumps(py_tree)
|
|
|
|
self.get_logger().info(f"Sending goal to action server...")
|
|
send_goal_future = self._action_client.send_goal_async(
|
|
goal_msg,
|
|
feedback_callback=self.feedback_callback
|
|
)
|
|
|
|
send_goal_future.add_done_callback(self.goal_response_callback)
|
|
|
|
def goal_response_callback(self, future):
|
|
goal_handle = future.result()
|
|
if not goal_handle.accepted:
|
|
self.get_logger().info('Goal rejected :(')
|
|
return
|
|
|
|
self.get_logger().info('Goal accepted :)')
|
|
|
|
self._get_result_future = goal_handle.get_result_async()
|
|
self._get_result_future.add_done_callback(self.get_result_callback)
|
|
|
|
def get_result_callback(self, future):
|
|
result = future.result().result
|
|
self.get_logger().info(f'Result: {{success: {result.success}, message: {result.message}}}')
|
|
# Optionally, you can broadcast the final result via WebSocket here
|
|
|
|
def feedback_callback(self, feedback_msg):
|
|
"""
|
|
This callback is triggered by the action server.
|
|
It forwards the status to the QGC plugin via the WebSocket manager in a thread-safe manner.
|
|
"""
|
|
feedback = feedback_msg.feedback
|
|
feedback_payload = json.dumps({"node_id": feedback.node_id, "status": feedback.status})
|
|
self.get_logger().info(f"Received feedback: {feedback_payload}")
|
|
websocket_manager.broadcast(feedback_payload)
|
|
|
|
# Note: The rclpy.init() and spinning of the node will be handled in main.py
|