optimize controller logic

.gitignore

@@ -0,0 +1 @@
+*.jpg

src/build/drone_controller/build/lib/drone_controller/config.py

@@ -18,7 +18,8 @@
 DEFAULT_YAW_RATE    = 0.5
 DEFAULT_VERTICAL_VEL    = 1.0
 
-SOCKET_IP           = '127.0.0.1'
+# SOCKET_IP           = '127.0.0.1'
+SOCKET_IP           = '10.42.0.1'
 SOCKET_PORT         = 8765
 
 GROUND_ALTITUDE     = 7.8
@@ -41,7 +42,10 @@
 DIRECTORY_PREFIX    = '/home/x650/UAV/USB_CAM/'
 
 MAX_CLIMB_HEIGHT        = 10.0
-PANEL_POS_THRES_X       = 80
-PANEL_POS_THRES_Y       = 50
+PANEL_POS_THRES_X       = 50
+PANEL_POS_THRES_Y       = 30
+PANEL_YAW_THRES         = 5
+
+STATIC_VELOCITY         = 0.001
 
 

src/build/drone_controller/build/lib/drone_controller/drone_controller.py

@@ -54,8 +54,8 @@ def preflight(self) -> bool:
         if self.state == None or not self.state.connected:
             return False
 
-        if self.state.mode == 'OFFBOARD' and self.state.armed:
-            return True
+        # if self.state.mode == 'OFFBOARD' and self.state.armed:
+        #     return True
 
         for _ in range(10):
             self.init_pose()
@@ -137,9 +137,6 @@ async def send_response(self, socket, response):
         except Exception as e:
             self.get_logger().info(f"Failed to send response: {e}")
 
-    def run_server(self):
-        asyncio.run(self.start_websocket_server())
-
     async def start_websocket_server(self):
         async with websockets.serve(self.websocket_handler, SOCKET_IP, SOCKET_PORT):
             self.get_logger().info(f"WebSocket server started at ws://{SOCKET_IP}:{SOCKET_PORT}")
@@ -180,4 +177,4 @@ def main(args=None):
             rclpy.shutdown()
 
 if __name__ == "__main__":
-    main()
+    main()

src/build/drone_controller/build/lib/drone_controller/socket_bridge.py

@@ -0,0 +1,174 @@
+import json
+import websockets
+import asyncio
+import rclpy
+import time
+import rclpy.logging
+
+from rclpy.node import Node
+from mavros_msgs.msg import State
+from drone_interfaces.srv import TaskDispatch
+from geometry_msgs.msg import PoseStamped, Point, Quaternion
+from std_msgs.msg import Header
+from mavros_msgs.srv import CommandBool, SetMode, CommandTOL
+
+from drone_controller.utils import *
+from drone_controller.constant import *
+from drone_controller.task.task_handler import TaskHandler
+
+
+class SocketBridge(Node):
+    def __init__(self):
+        super().__init__('socket_bridge')
+
+        self.pose_pub = self.create_publisher(PoseStamped, '/mavros/setpoint_position/local', 10)
+        self.state_sub = self.create_subscription(State, '/mavros/state', self.state_callback, 10)
+
+        self.task_handler = TaskHandler()
+        self.state = None
+        self.pending_task = []
+
+        self.task_client = self.create_client(TaskDispatch, '/drone/dispatch_task')
+        self.arming_client = self.create_client(CommandBool, '/mavros/cmd/arming')
+        self.mode_client = self.create_client(SetMode, '/mavros/set_mode')
+
+        self.takeoff_client = self.create_client(CommandTOL, '/mavros/cmd/takeoff')
+        self.land_client = self.create_client(CommandTOL, '/mavros/cmd/land')
+
+        while not self.task_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("Waiting for TaskDispatch service to be available...")
+
+        while not self.arming_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Arming service not available, waiting...')
+
+        while not self.mode_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Mode service not available, waiting...')
+
+        self.get_logger().info('All services are available, ready to arm and take off.')
+
+
+    def state_callback(self, msg: State):
+        self.state = msg
+
+    def preflight(self) -> bool:
+        rclpy.spin_once(self)
+        if self.state == None or not self.state.connected:
+            return False
+
+        # if self.state.mode == 'OFFBOARD' and self.state.armed:
+        #     return True
+
+        for _ in range(10):
+            self.init_pose()
+            res = self.set_mode('OFFBOARD')
+            if res:
+                break
+            time.sleep(2)
+        if not res:
+            return False
+
+        return self.arm_drone()
+
+    def init_pose(self, time_sec=1.5):
+        waypoint = PoseStamped()
+        waypoint.header = Header()
+        waypoint.header.stamp = self.get_clock().now().to_msg()
+        waypoint.pose.position = Point(x=0.0, y=0.0, z=0.0)
+        waypoint.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+
+        iter = int(time_sec / 0.05)
+        for _ in range(iter):
+            self.pose_pub.publish(waypoint)
+            time.sleep(0.05)
+
+    def send_task_request(self, task) -> bool:
+        request = TaskDispatch.Request()
+        request.task = task
+        future = self.task_client.call_async(request)
+
+        while not future.done():
+            self.init_pose(0.5)
+            rclpy.spin_once(self)
+
+        res = future.result().success
+        if res:
+            self.get_logger().info('Task Dispatched successfully.')
+        else:
+            self.get_logger().info('Failed to dispatch task.')
+
+        return res
+
+    def arm_drone(self) -> bool:
+        req = CommandBool.Request()
+        req.value = True
+        future = self.arming_client.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        if future.result().success:
+            self.get_logger().info('Drone armed successfully.')
+            return True
+        else:
+            self.get_logger().error('Failed to arm drone.')
+            return False
+
+    def disarm_drone(self):
+        req = CommandBool.Request()
+        req.value = False
+        future = self.arming_client.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        if future.result().success:
+            self.get_logger().info('Drone disarmed successfully.')
+        else:
+            self.get_logger().error('Failed to disarm drone.')
+
+    def set_mode(self, mode) -> bool:
+        req = SetMode.Request()
+        req.custom_mode = mode
+        future = self.mode_client.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        if future.result().mode_sent:
+            self.get_logger().info(f'Mode set to {mode} successfully.')
+            return True
+        else:
+            self.get_logger().error(f'Failed to set mode to {mode}.')
+            return False
+
+    async def send_response(self, socket, response):
+        try:
+            await socket.send(json.dumps(response))
+        except Exception as e:
+            self.get_logger().info(f"Failed to send response: {e}")
+
+    async def start_websocket_server(self):
+        async with websockets.serve(self.websocket_handler, SOCKET_IP, SOCKET_PORT):
+            self.get_logger().info(f"WebSocket server started at ws://{SOCKET_IP}:{SOCKET_PORT}")
+            await asyncio.Future() 
+
+    async def websocket_handler(self, websocket, path):
+        async for message in websocket:
+            self.get_logger().info(f'receive websocket message: {message}')
+            task, response = self.task_handler.handle_json_data(message, websocket)
+            if response == None:
+                self.pending_task.append(task)
+                response = {"status": "success", "message": "Task dispatched successfully"}
+            await self.send_response(websocket, response)
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    try:
+        socket_bridge = SocketBridge()
+        socket_bridge.get_logger().info("SocketBridge node initialized.")
+
+        asyncio.run(socket_bridge.start_websocket_server())
+
+    except KeyboardInterrupt:
+        socket_bridge.get_logger().info("Shutting down SocketBridge node.")
+    except Exception as e:
+        socket_bridge.get_logger().info(f"Unhandled exception: {e}")
+    finally:
+        if rclpy.ok():
+            socket_bridge.destroy_node()
+            rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

src/build/drone_controller/build/lib/drone_controller/task/task_executor.py

@@ -3,14 +3,15 @@
 
 from queue import Queue
 from rclpy.node import Node
-from drone_interfaces.srv import TaskDispatch
-from drone_interfaces.msg import Task, RawWaypoint
+from drone_interfaces.srv import TaskDispatch, YoloRequest
+from drone_interfaces.msg import Task, RawWaypoint, TaskState
 from drone_interfaces.action import ExecuteWaypoint
 from rclpy.action import ActionClient
 from std_msgs.msg import Float64
 from geometry_msgs.msg import PoseStamped, Vector3
 from sensor_msgs.msg import NavSatFix, NavSatStatus
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+from mavros_msgs.srv import CommandTOL
 
 from drone_controller.utils import *
 from drone_controller.constant import *
@@ -20,6 +21,16 @@ class TaskExecutor(Node):
     def __init__(self):
         super().__init__('task_executor')
 
+        self.state_pub = self.create_publisher(TaskState, 'drone/task_state', 10)
+
+        self.land_client = self.create_client(CommandTOL, '/mavros/cmd/land')
+        self.yolo_client = self.create_client(YoloRequest, '/drone/yolo_request')
+
+        while not self.land_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("Waiting for Land service to be available...")
+        while not self.yolo_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info("Waiting for Yolo service to be available...")
+
         qos_profile = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=10)
         self.gps_sub = self.create_subscription(NavSatFix, '/mavros/global_position/global', self.gps_callback, qos_profile)
         self.yaw_sub = self.create_subscription(Float64, '/mavros/global_position/compass_hdg', self.yaw_callback, qos_profile)
@@ -65,6 +76,29 @@ def handle_task_request(self, request, response):
             response.success = False
 
         return response
+
+    def yolo_request(self, start: bool):
+        req = YoloRequest.Request()
+        req.start = start
+        future = self.yolo_client.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        if future.result().success:
+            self.get_logger().info('Request yolo service successfully.')
+        else:
+            self.get_logger().error('Failed to request yolo service.')
+
+    def land(self):
+        rclpy.spin_once(self)
+        req = CommandTOL.Request()
+        req.latitude = self.gps_fix.latitude
+        req.longitude = self.gps_fix.longitude
+        future = self.land_client.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        if future.result().success:
+            self.get_logger().info('Drone landed successfully.')
+            self.has_takeoff = False
+        else:
+            self.get_logger().error('Failed to land.')
 
     def execute_takeoff(self, id, altitude=DEFAULT_TAKEOFF_ALTITUDE):
         rclpy.spin_once(self)
@@ -133,8 +167,11 @@ def process_pending_tasks(self):
         """
         while rclpy.ok():
             rclpy.spin_once(self)
+            # self.state_pub.publish(TaskState(state=1))
 
             if self.pending_tasks.qsize() > 0:
+                self.yolo_request(True)
+
                 task = self.pending_tasks.get()
 
                 next_waypoint_id = 0
@@ -150,7 +187,6 @@ def process_pending_tasks(self):
                     next_waypoint_id += 1
 
                 while index < length:
-                    self.get_logger().info(f'{length, index}')
                     waypoint = task.waypoints[index]
                     waypoint.id = next_waypoint_id
                     self.send_waypoint_action(waypoint)
@@ -162,13 +198,13 @@ def process_pending_tasks(self):
 
                     index += 1
 
-                if self.has_takeoff:
-                    self.execute_land(next_waypoint_id)
-                    next_waypoint_id += 1
-
+                self.yolo_request(False)
                 self.get_logger().info(f"Task completed.")
             else:
-                time.sleep(1.0)
+                if self.has_takeoff:
+                    # self.execute_land(next_waypoint_id)
+                    # next_waypoint_id += 1
+                    self.land()
 
     def send_waypoint_action(self, waypoint):
         """
@@ -185,7 +221,8 @@ def send_waypoint_action(self, waypoint):
         goal_future.add_done_callback(self.goal_response_callback)
 
         while not self.goal_finished:
-            rclpy.spin_once(self, timeout_sec=0.1)
+            rclpy.spin_once(self)
+            # self.state_pub.publish(TaskState(state=0))
 
         self.goal_finished = False
 

src/build/drone_controller/build/lib/drone_controller/task/task_handler.py

@@ -113,7 +113,7 @@ def handle_json_data(self, json_data, socket):
         try:
             # Parse the task
             raw_task = json_data
-            if not isinstance(json_data, dict):
+            if not isinstance(json_data, (dict, list)):
                 raw_task = json.loads(json_data)
 
             task = self.parse_task(raw_task)