class MainAutoControll: missionCords = [] def __init__(self, communicationMode=None, logLevel=logging.DEBUG, logFile='logs.log'): self.isSuccessfullyInitialized = True try: self.io = IO(*communicationMode, storagePath="storage\\", storageFilename=None, logFile=logFile, logLevel=logLevel) self.io.loggerWrapper.logInfo("MainAutoControll ctor started") self.initGpsPathFinder() except Exception as e: # todo: error handling! self.isSuccessfullyInitialized = False self.io.loggerWrapper.logError("Unknown error during autoRover starting: " + str(e)) def initGpsPathFinder(self): # gps path finder self.gpsPathFinder = GpsPathFinder("map.osm", "foot") # ustalanie trasy dla danego przejazdu self.gpsPathFinder.definePath([50.01824, 21.98584], [50.01837921142578, 21.986557006835984]) self.routeOSM, self.routeLocation = self.gpsPathFinder.GetRouteToTravel() # todo: MOVE TO GPS SERVICE! def moveByGPS(self, gps): # Here is place to get current location from gps lat = gps[1][1] lon = gps[1][0] print("lat: " + str(lat) + " | lon: " + str(lon)) nearestNode = self.gpsPathFinder.GetNearestNodeToCurrentLocation([float(lat), float(lon)]) try: gpsSensorResult = self.gpsPathFinder.CheckCurrentPositionAtRoute([lat, lon], nearestNode) if gpsSensorResult.status == ResultStatus.Success: distanceToGoalPoint = gpsSensorResult.data[0] distanceToNearestNode = gpsSensorResult.data[1] angle = gpsSensorResult.data[2] print("Arithmetic Distance to goal is" + str(distanceToGoalPoint)) print("Distance to current nearest node in road is ", distanceToNearestNode) print("angle: " + str(angle)) angle *= 5000 # Temp TODO MOVE DECISION OF ROVER DRIVING TO "COMMON SENSOR GUARDIAN" (Name is temp) isGoalReached = distanceToGoalPoint <= 0.00004 if not isGoalReached: print("Goal is not reached") self.io.communicationService.sendPacket(PacketsSend.RightLeftDriving, ( 50.0 - 50 * angle, 55.0 + 50 * angle)) # right/left levels (-100, 100) except Exception as e: # todo: error handling! print(e) def mainLoop(self): if not self.isSuccessfullyInitialized: return while True: time.sleep(0.001) if not self.io.communicationService.isCommunicationRunning: if not self.io.start(communication=True, storage=not self.io.storageService.isStorageInitialized()): self.io.loggerWrapper.logError("Cannot connect to rover!") continue self.io.communicationService.sendPacket(PacketsSend.RightLeftDriving, (50.0, 55.0)) # right/left levels (-100, 100) gps = self.io.readGPS() if gps is None: continue else: self.moveByGPS(gps)