代码内容转移

backend_service/src/ros2_client.py

@@ -0,0 +1,67 @@
+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