Initial commit

Lts/Proxies/GuezNasat/Simulator/nasat_socket.py

@@ -0,0 +1,337 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Fri Dec 11 13:40:11 2020
+
+@author: Filip Biedziak
+"""
+
+import os
+import sys
+import websocket
+import threading
+import time
+import math
+import json
+
+from collections import OrderedDict
+from constants import HOST, PORT, NASAT_TELEGRAM_TYPES, LAT_BRE, LON_BRE, LAT_LTZ, LON_LTZ, R, Colors
+
+
+class NasatSocket(object):
+    
+    _websocket = None
+    _host = HOST
+    _port = PORT
+    _status = False
+    _statusThread = None
+    _checkStatus = True
+    _interrupt = True
+    _verbose = 0
+    _telegram = OrderedDict()
+    _buffer = []
+    _aircraftId = None
+    _location = None
+    _telegramType = None
+    _timestamp = 0
+    _position = OrderedDict()
+    _phi = 0.0
+    _dPhi = 6.0 * math.pi / 360.0
+    _lon = None
+    _lat = None
+    _fuseType = None
+    _muncode = None
+    _flightTime = None
+
+
+    def __init__(self, telegramType, verbose):
+        """Constructor."""
+        assert isinstance(telegramType, str)
+        assert isinstance(verbose, int)
+
+        if telegramType not in NASAT_TELEGRAM_TYPES:
+            self.error("Unknown telegram type: %s" % telegramType)
+        self._telegramType = telegramType
+        self._telegram["type"] = self._telegramType
+        self._telegram["data"] = OrderedDict()
+        self._telegram["data"]["aircraftId"] = self._aircraftId
+        
+        self._verbose = verbose
+
+
+    def error(self, msg):
+        print("%s%sERROR%s: %s" % (Colors.BOLD, Colors.RED, Colors.RESET, msg))
+        self.close()
+        sys.exit(0)
+
+
+    def host(self):
+        return self._host
+
+    def setHost(self, host=None):
+        assert isinstance(host, str) 
+        self._host = host
+
+
+    def port(self):
+        return self._port
+
+    def setPort(self, port=None):
+        assert isinstance(port, int) 
+        self._port = port
+
+    def status(self):
+        return self._status
+
+    def checkStatus(self):
+        return self._checkStatus
+
+    def setCheckStatus(self, check):
+        assert isinstance(check, bool) 
+        self._checkStatus = check
+    
+    def interrupt(self):
+        return self._interrupt
+    
+    def setInterrupt(self, interrupt):
+        assert isinstance(interrupt, bool)
+        self._interrupt = interrupt
+
+
+    def aircraftId(self):
+        return self._aircraftId
+
+    def setAircraftId(self, aircraftId):
+        assert isinstance(aircraftId, int)
+        if aircraftId < 0:
+            self.error("Invalid aircraft ID: %d" % aircraftId)
+        self._aircraftId = aircraftId
+        self._telegram["data"]["aircraftId"] = self._aircraftId
+
+
+    def telegramType(self):
+        return self._telegramType
+
+    def setPosition(self, lon, lat):
+        self._lon = lon
+        self._lat = lat
+        
+    def setMunition(self, muncode):
+        self._muncode = muncode
+    
+    def setFuseType(self, fuseType):
+        self._fuseType = fuseType
+
+    def setFlightTime(self, flightTime):
+        self._flightTime = flightTime
+
+
+    def location(self):
+        return self._location
+
+    def setLocation(self, location):
+        assert isinstance(location, str)
+        if location not in ["Bremen", "Letzlingen"]:
+            self.error("Unknown location: %s" % location)
+        self._location = location
+
+    # ==================================================== EXCON Connection ====================================================
+    def checkConnectionStatus(self):
+        startTime = time.time()
+        while self._checkStatus:
+            changed = False
+            
+            # check if still connected
+            if self._status:
+                try:
+                    self._websocket.send("")  # send an empty message
+                except:
+                    self._status = False
+                    changed = True
+
+            # attempt to reconnect
+            else:
+                if self._verbose > 1:
+                    print("Connecting to %s:%d" % (self._host, self._port))
+                try:
+                    self._websocket = websocket.create_connection("%s:%d" % (self._host, self._port))
+                    self._status = True
+                    changed = True
+                except:
+                    pass
+
+            # only show when status changes
+            if changed and self._verbose > 0:
+                color = Colors.GREEN if self._status else Colors.RED
+                status = "OK" if self._status else "not connected"
+                print("Connection status: %s%s%s%s" % (Colors.BOLD, color, status, Colors.RESET))
+
+            # check every second
+            time.sleep(1.0 - ((time.time() - startTime) % 1.0))
+
+    def connect(self):
+        self._statusThread = threading.Thread(target=self.checkConnectionStatus)
+        self._statusThread.start()
+
+    def close(self):
+        if self._verbose > 0:
+            print("Closing connection.")
+        self._checkStatus = False
+        self._status = False
+        if self._statusThread is not None:
+            self._statusThread.do_run = False
+        if self._websocket is not None:
+            self._websocket.close()
+
+    # ===================================================== NASAT Telegrams ====================================================
+    def start(self, **kwargs):
+        if self._verbose > 1:
+            print("Sending telegram of type: %s" % self._telegramType)
+        if self._telegramType == "NasatPosition":
+            if self._aircraftId is None:
+                self.error("Aircraft ID has not been set.")
+            elif self._location is None:
+                self.error("Location has not been set.")
+            self.simulateNasatPosition()
+        elif self._telegramType == "NasatNineLiner":
+            self.simulateNineLiner(**kwargs)
+        elif self._telegramType == "NasatWeaponRelease":
+            self.simulateWeaponRelease(**kwargs)
+        else:
+            self.error("Unknown telegram type: %s" % self._telegramType)
+
+    def send(self):
+        self._timestamp = time.time()
+        self._telegram["data"]["timestamp"] = self._timestamp
+        
+      #  print("send Telegram: %s" % self._telegram)
+        telegram = json.dumps(self._telegram)
+        if self._status:
+            try:
+                if self._verbose > 1:
+                    print("Sending telegram: %s" % telegram)
+                self._websocket.send(telegram)
+            except:
+                self._buffer.append(telegram)  # just in case of timing issues
+        else:
+            self._buffer.append(telegram)
+
+    def sendBuffer(self):
+        bufferCopy = [telegram for telegram in self._buffer]
+        for telegram in bufferCopy:
+            try:
+                self._websocket.send(telegram)
+                self._buffer = self._buffer[1:]
+                time.sleep(0.1)
+            except:
+                break
+
+    # ====================================================== NasatPosition =====================================================
+    def calculatePosition(self):
+        lat = LAT_BRE if self._location == "Bremen" else LAT_LTZ if self._location == "Letzlingen" else 0
+        lon = LON_BRE if self._location == "Bremen" else LON_LTZ if self._location == "Letzlingen" else 0
+        self._position["lat"] = lat + R * math.cos(self._phi)
+        self._position["lon"] = lon + R * math.sin(self._phi) * 1.7  # roughly a circle
+        self._position["height"] = 1000
+
+        self._telegram["data"]["position"] = self._position
+
+        self._phi += self._dPhi
+
+    def simulateNasatPosition(self):
+        startTime = time.time()
+        count = 0
+        while True:
+            if self._status and len(self._buffer) > 0:
+                self.sendBuffer()
+
+            self.calculatePosition()  # update current telegram
+            self.send()
+
+            time.sleep(1.0 - ((time.time() - startTime) % 1.0))
+            
+            if self._interrupt:
+                if count == 10:
+                    self.close()
+                elif count == 20:
+                    self.setCheckStatus(True)
+                    self.connect()
+                count += 1
+
+    # ===================================================== NasatNineLiner =====================================================
+    def readNineLinerData(self, fname):
+        assert isinstance(fname, str)
+        
+        if not os.path.exists(fname):
+            self.error("NasatNineLiner telegrams requires a JSON file with additional data.")
+        try:
+            with open(fname, 'r') as f:
+                data = json.load(f)
+        except:
+            self.error("Unable to read Nine Liner data from JSON file: %s" % fname)
+        if data["data"]["timestamp"] == -1:
+            data["data"]["timestamp"] = time.time()
+        return data
+
+    def simulateNineLiner(self, **kwargs):
+        if "fname" not in kwargs.keys():
+            self.error("Missing argument 'fname' with path to Nine Liner data.")
+
+        self._telegram = self.readNineLinerData(kwargs["fname"])
+        
+        startTime = time.time()
+        while not self._status:
+            time.sleep(1.0 - ((time.time() - startTime) % 1.0))  # wait until connection has been established
+            
+        self.send()
+        self.close()
+
+    # ================================================= NasatWeaponRelease =================================================
+    def readWeaponReleaseData(self, fname):
+        assert isinstance(fname, str)
+
+        if not os.path.exists(fname):
+            self.error("NasatWeaponRelease telegrams requires a JSON file with additional data. %s" % fname)
+        try:
+            with open(fname, 'r') as f:
+                data = json.load(f)
+        except:
+            self.error("Unable to read NasatWeaponRelease data from JSON file: %s" % fname)
+        if data["data"]["timestamp"] == -1:
+            data["data"]["timestamp"] = time.time()
+
+        return data
+
+    def simulateWeaponRelease(self, **kwargs):
+        if "fname" not in kwargs.keys():
+            self.error("Missing argument 'fname' with path to Weapon Release data.")
+
+        self._telegram = self.readWeaponReleaseData(kwargs["fname"])
+        if (self._lon is not None and self._lat is not None):
+            print("REPLACE pos")
+            self._telegram["data"]["impactPosition"]["lat"] = self._lat
+            self._telegram["data"]["impactPosition"]["lon"] = self._lon
+        if (self._muncode is not None):
+            print("Replace muncode")
+            self._telegram["data"]["munition"] = self._muncode
+
+        if (self._fuseType is not None):
+            print("Replace fusetype")
+            self._telegram["data"]["fuseType"] = self._fuseType
+        if (self._flightTime is not None):
+            print("Replace flightTime")
+            self._telegram["data"]["flightTime"] = self._flightTime
+        if (self._aircraftId is not None):
+            print("Replace aircraft")
+            self._telegram["data"]["aircraftId"] = self._aircraftId
+                        
+            
+            
+
+        startTime = time.time()
+        while not self._status:
+            time.sleep(1.0 - ((time.time() - startTime) % 1.0))  # wait until connection has been established
+
+        print("send NasatWeaponRelease with timestamp: %s" % self._telegram)
+        self.send()
+        self.close()