Add socket cleanup

tello_sim/command_server.py

@@ -1,7 +1,6 @@
 import os
 import socket
 from ursina import *
-from cv2.typing import MatLike
 from time import time
 import cv2
 from ursina_adapter import UrsinaAdapter
@@ -17,10 +16,34 @@ class CommandServer:
         self.stream_active = False
         self.last_altitude = 0
         self._recording_folder = "output/recordings"
+        self.server_socket = None
         
         if not os.path.exists(self._recording_folder):
             os.makedirs(self._recording_folder)
 
+    def check_port_available(self, port: int = 9999) -> bool:
+        """
+        Check if the specified port is available.
+        Returns True if available, False if in use.
+        """
+        try:
+            test_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            test_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+            test_socket.bind(('localhost', port))
+            test_socket.close()
+            return True
+        except OSError:
+            return False
+
+    def cleanup(self):
+        """Clean up resources and close the server socket."""
+        if self.server_socket:
+            try:
+                self.server_socket.close()
+                print("[Command Listener] Server socket closed.")
+            except Exception as e:
+                print(f"[Command Listener] Error closing socket: {e}")
+
     def streamon(self):
         """Start capturing screenshots and enable FPV video preview."""
         if not self.stream_active:
@@ -60,16 +83,35 @@ class CommandServer:
         """
         Listens for commands to send to the Simulator
         """
-        server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-        server.bind(('localhost', 9999))  # Port number for communication
+        self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
-        server.listen(5)
-        print("[Command Listener] Listening on port 9999...")
         
-        while True:
+        try:
-            conn, _ = server.accept()
+            self.server_socket.bind(('localhost', 9999))  # Port number for communication
-            data = conn.recv(1024).decode()
+            self.server_socket.listen(5)
-            if data:
+            print("[Command Listener] Listening on port 9999...")
-                print(f"[Command Listener] Received command: {data}")
+        except OSError as e:
+            if e.errno == 48:  # Address already in use
+                print("\n" + "="*70)
+                print("ERROR: Port 9999 is already in use!")
+                print("="*70)
+                print("\nAnother instance of the simulator may be running.")
+                print("\nTo fix this, run one of these commands in your terminal:")
+                print("  macOS/Linux: lsof -ti:9999 | xargs kill -9")
+                print("  Windows:     netstat -ano | findstr :9999")
+                print("               taskkill /PID <PID> /F")
+                print("\nOr simply restart your computer.")
+                print("="*70 + "\n")
+                raise
+            else:
+                raise
+
+        try:
+            while True:
+                conn, _ = self.server_socket.accept()
+                data = conn.recv(1024).decode()
+                if data:
+                    print(f"[Command Listener] Received command: {data}")
                 
                 if data == "connect":
                     self._ursina_adapter.connect()
@@ -221,4 +263,11 @@ class CommandServer:
                 elif data == "end":
                     self.end()
 
-            conn.close()
+                conn.close()
+        except KeyboardInterrupt:
+            print("\n[Command Listener] Shutting down...")
+            self.cleanup()
+        except Exception as e:
+            print(f"[Command Listener] Error: {e}")
+            self.cleanup()
+            raise

tello_sim/tello_drone_sim.py

@@ -1,21 +1,63 @@
 from command_server import CommandServer
 from ursina_adapter import UrsinaAdapter
 import threading
+import atexit
+import signal
+import sys
 
 class TelloDroneSim:
     def __init__(self):
         self._ursina_adapter = UrsinaAdapter()
         self._server = CommandServer(self._ursina_adapter)
         
+        # Register cleanup handlers
+        atexit.register(self.cleanup)
+        signal.signal(signal.SIGINT, self._signal_handler)
+        signal.signal(signal.SIGTERM, self._signal_handler)
+
+    def _signal_handler(self, signum, frame):
+        """Handle termination signals gracefully."""
+        print("\n[Tello Sim] Received shutdown signal, cleaning up...")
+        self.cleanup()
+        sys.exit(0)
+
+    def cleanup(self):
+        """Clean up resources."""
+        if hasattr(self, '_server'):
+            self._server.cleanup()
+
     @property
     def state(self):
         return self._ursina_adapter
 
     def start(self):
+        # Check if port is available before starting
+        if not self._server.check_port_available(9999):
+            print("\n" + "="*70)
+            print("ERROR: Cannot start simulator - Port 9999 is already in use!")
+            print("="*70)
+            print("\nAnother instance of the simulator may be running.")
+            print("\nTo fix this, run one of these commands in your terminal:")
+            print("  macOS/Linux: lsof -ti:9999 | xargs kill -9")
+            print("  Windows:     netstat -ano | findstr :9999")
+            print("               taskkill /PID <PID> /F")
+            print("\nOr simply restart your computer.")
+            print("="*70 + "\n")
+            sys.exit(1)
+        
         server_thread = threading.Thread(target=self._server.listen)
         server_thread.daemon = True
         server_thread.start()
-        self._ursina_adapter.run()
+        
+        try:
+            self._ursina_adapter.run()
+        except KeyboardInterrupt:
+            print("\n[Tello Sim] Interrupted, cleaning up...")
+            self.cleanup()
+        except Exception as e:
+            print(f"[Tello Sim] Error: {e}")
+            self.cleanup()
+            raise
 
     def update(self) -> None:
         self._ursina_adapter.tick()