# -*- coding: cp1252 -*- ## @package PltfCtrl # Python-Bibliothek, die die Kommunikation mit PlatformCtrl kapselt. # Python Library to communicate with PltfCtrl, and to control a Platform. # # ----------------------------------------------------------------------------------- # # Copyright (C) 2010 Neobotix (www.neobotix.de) # # This software is allowed to be used and modified only in association with a Neobotix # robot platform. It is allowed to include the software into applications and # to distribute it with a Neobotix robot platform. # # This software is provided WITHOUT ANY WARRANTY; without even the # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR # PURPOSE. See the Neobotix License (Version 1.0) for more details. # # You should have received a copy of the Neobotix License # along with this software; if not, write to the # Gesellschaft fuer Produktionssysteme, Neobotix, Nobelstrasse 12, 70569 Stuttgart, Germany # # ----------------------------------------------------------------------------------- from Neobotix.Bases.Message import Message from Neobotix.Bases.SocketClient import SocketClient from Neobotix.Nav.Map import Map2D, RoadMap from Neobotix.Pltf.Comm.EnumMsgPltfCtrl import EnMsgPltfCtrl, PltfParams, \ BoardType from Neobotix.Pltf.Data.StatePltf import StatePltf from threading import Thread from time import sleep, ctime class PltfCtrlClient(Thread): def __init__(self): self.socketClient = None self.__connected = False self.__requestTime = 0.1 self.statePltf = StatePltf() self.__connectionListeners = [] Thread.__init__(self, None, None, 'PltfClientThread') self.setDaemon(True) self.start() def onConnect(self): self.__connected = True for listener in self.__connectionListeners: listener.onConnect() def onDisconnect(self): self.__connected = False self.socketClient = None for listener in self.__connectionListeners: listener.onDisconnect() def onReceive(self, msg): type = msg.getMessageType() if type == EnMsgPltfCtrl.CMD_GET_STATUS: self.statePltf.updateStateFromMessage(msg) if type == EnMsgPltfCtrl.CMD_GET_DEBUG_DATA: pass for listener in self.__connectionListeners: listener.onReceive(msg) def run(self): while (True): if self.__connected: #Send request Status self.sendCmd(EnMsgPltfCtrl.CMD_GET_STATUS) #Yield sleep(self.__requestTime) ####################### # Connection Methods # ####################### ## Verbindung zu einem PlatformCtrl-Server aufbauen - Connect to a PlatformCtrl server # @param server Die IP-Adresse des Servers als STRING - The IP-address of the server as STRING # @param port Optional: Der Port des Servers als INT - Optional: The port of the server as INT # @param requesttime Optional: Pause zwischen zwei Statusabfragen in Millisekunden als INT - Optional: Pause between status requests in [ms] as INT # # Stellt eine Verbindung zu PLatformCtrl her (Standard-Port ist 10002, Standard-Zykluszeit is 100ms). - # Connect to PlatformCtrl (standard port is 10002, standard cycle is 100 ms) def connect(self, address, port = 10002, requestTime = 200): self.__requestTime = requestTime/1000. #Socket anlegen - Create the socket self.socketClient = SocketClient() #Listener anlegen (muss onConnect und onDisconnect implementieren) - Listen to the socket connection status, must implement onConnect / onDisconnect self.socketClient.addConnectionListener(self) #Verbinden - Connect self.socketClient.connect(address, port) return True ## Verbindung abbauen - Close connection by asking server to disconnect. # def disconnect(self): self.socketClient.close() ## @deprecated: Verbindung zum Server abbauen - Close connection to server. # def closeConnection(self): self.disconnect() ## Schickt bei bestehender Verbindung ein Kommando - Sends a simple command if connected # # Schickt einen Befehl aus der Neobotix.Pltf.Comm.EnumMsgPltfCtrl.EnMsgPltfCtrl() Liste. - # Sends a command from the Neobotix.Pltf.Comm.EnumMsgPltfCtrl.EnMsgPltfCtrl() Enumeration. def sendCmd(self, cmd): msg = Message() msg.writeByte(cmd) #Feststellen, ob der Befehl eine Antwort erfordert - Decide if the cmd has a reply if cmd == EnMsgPltfCtrl.CMD_GET_STATUS: msg.setHasReply(True) elif cmd == EnMsgPltfCtrl.CMD_GET_DEBUG_DATA: msg.setHasReply(True) else: msg.setHasReply(False) msg.setMessageType(cmd) self.socketClient.addToSendQueue(msg) ####################### # Status Methods # ####################### ## Prüft die Verbindung zum Server - Checks the connection to the server # @return True, wenn verbunden - True If connected # @return False, wenn nicht verbunden - False if not connected # # Funktion zum Prüfen, ob eine Verbindung zum PlatformCtrl-Server besteht. - Function to check if a connection to a server has been established. def isConnected(self): return self.__connected ## Fährt den Bordrechner der Plattform herunter - Shuts the platform host computer down # @return none # # Fährt den Bordrechner der mobilen Plattform herunter, falls dieser Windows verwendet. Bei Verwendung von Linux mit # schreibgeschütztem Betriebssystem normalerweise nicht mehr notwendig. - # Command to shut down the host computer with PlatformCtrl. Only for older platforms running on windows. # It is not necessary anymore due to a read-only filesystem. def shutdownHost(self): self.sendCmd(EnMsgPltfCtrl.CMD_SHUTDOWN_HOST) ## Beendet den Bewegungs-Thread - Stops the motion thread # @return none # # Beendet den Bewegungs-Thread, um auf schwächeren Systemen Rechenleistung zu sparen. - # Stops the motion thread of PlatformCtrl. Used to save computation time on slow systems. def stopMotionThread(self): self.sendCmd(EnMsgPltfCtrl.CMD_STOP_MOTIONTHREAD) ## Startet den Bewegungs-Thread erneut - Restarts the motion thread # @return none # # Startet den Bewegungs-Thread erneut - Restarts the motion thread of PlatformCtrl def restartMotionThread(self): self.sendCmd(EnMsgPltfCtrl.CMD_RESTART_MOTIONTHREAD) ## Setzt einen Plattform-Parameter - Sets a olatform parameter # @param parameter Ein Parametername aus der Neobotix.Pltf.Comm.EnumMsgPltfCtrl.PltfParams Liste - A parameter selected from the Neobotix.Pltf.Comm.EnumMsgPltfCtrl.PltfParams enumeration. # @param value Ein STRING mit dem neuen Wert - A string containing the new value # # Basisfunktion um zur Laufzeit Parameter der Plattform zu ändern. Mögliche Parameter sind in Neobotix.Pltf.Comm.EnumMsgPltfCtrl.PltfParams definiert. - # Low-level function to set a platform parameter at runtime. Available parameters are stored in Neobotix.Pltf.Comm.EnumMsgPltfCtrl.PltfParams. def setParameter(self, parameter, value): msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_SET_PLTF_PARAM) msg.writeString(parameter[0]) msg.writeString(value) msg.writeByte(parameter[1]) msg.writeByte(parameter[2]) msg.writeInt(0) msg.writeInt(0) msg.setHasReply(False) msg.setMessageType(EnMsgPltfCtrl.CMD_SET_PLTF_PARAM) self.socketClient.addToSendQueue(msg) ################## # Motion Control # ################## ## Wählt einen Bewegungsmodus aus - Sets a motion mode # @param mode Ein Modus aus der EnumMsgPltfCtrl.MotionModes Liste - A mode frome the EnumMsgPltfCtrl.MotionModes enumeration # # Wählt einen neuen Bewegungsmodus für die PLattform aus und wartet, bis dieser angenommen wurde. Verfügbare Modi sind in EnumMsgPltfCtrl.MotionModes definiert. - # Sets a motion mode for the platform and waits until this new mode has been accepted. Available motion modes are defined in the EnumMsgPltfCtrl.MotionModes Enumeration. def setMotionMode(self, mode): self.stopPlatform() msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_SET_MODE) msg.writeInt(mode) msg.setHasReply(False) msg.setMessageType(EnMsgPltfCtrl.CMD_SET_MODE) self.socketClient.addToSendQueue(msg) while not self.isMotionModeSet(mode): sleep(0.5) self.sendCmd(EnMsgPltfCtrl.CMD_START) ## Prüft, ob ein bestimmter Bewegungsmodus ausgewählt wurde - Checks if a motion mode has been set # @param mode Der zu prüfenden Modus als INT - The motion mode to verify as INT # @return True, wenn der Modus akzeptiert wurde - True if motion mode is set # @return False, wenn der Modus nicht gesetzt wurde - False if mode is not set # # Prüft, ob ein bestimmter Bewegungsmodus ausgewählt wurde - Checks if a motion mode has been set def isMotionModeSet(self, mode): if self.statePltf.pltfData.motionMode is mode: return True else: return False ## Hält die Plattform sofort an - Stop platform motion # # Schickt ein Stopp-Kommando an die Plattform, die über die in Interpolation.ini angegebene Bremsrampe verzögert. - # Sends a stop command to PlatformCtrl. The platform decelerates with the ramp defined in Interpolation.ini def stopPlatform(self): self.sendCmd(EnMsgPltfCtrl.CMD_STOP) ## Fährt eine Zielstation an (nicht blockierend) - Move to a station (non blocking) # @param station Der Name der Zielstation als STRING - The station's name as STRING # @return True, wenn ein Pfad geplant wurde und die Plattform fährt - True if planning was successful and movement has started # @return False im Fehlerfall - False if an error occured # # Schickt den Befehl, die angegebenen Zielstation anzufahren. Die Funktion prüft, ob die Planung erfolgreich war und die Plattform fährt # (Wartezeit maximal 15s). - # Sends the command to move to a roadmap station. The function checks if planning was successful and movement has started (timeout is 15 seconds) def moveToStationNonBlocking(self, station): self.__moveToStation(station) self.sendCmd(EnMsgPltfCtrl.CMD_START) timeout = 15 while self.statePltf.pltfData.pltfTargetReached: sleep(0.5) timeout = timeout -1 if (timeout < 0): return False return True ## Fährt eine Zielstation an (blockierend) - Move to a station (blocking) # @param station Der Name der Zielstation als STRING - The station's name as STRING # # Schickt den Befehl, die angegebenen Zielstation anzufahren. Die Funktion blockiert, bis das Ziel erreicht wurde oder die zulässige Zeit # überschritten wurde. - # Sends the command to move to a roadmap station. Function blocks until the target has been reached without timeout. def moveToStationBlocking(self, station): self.__moveToStation(station) self.sendCmd(EnMsgPltfCtrl.CMD_START) while self.statePltf.pltfData.pltfTargetReached: sleep(0.5) while not self.statePltf.pltfData.pltfTargetReached: sleep(0.5) print str(ctime()) + " target " + station + " reached" def __moveToStation(self, station): msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_MOVE_TO_STATION) msg.writeString(station) msg.setHasReply(False) msg.setMessageType(EnMsgPltfCtrl.CMD_MOVE_TO_STATION) self.socketClient.addToSendQueue(msg) ## Fährt eine Zielposition in der Karte an (nicht blockierend) - Move to a position in the map (non blocking) # @param x X-Koordinate im Weltkoordinatensystem in Millimetern als INT - X-coordinate in the world coordinate system in mm as INT # @param y Y-Koordinate im Weltkoordinatensystem in Millimetern als INT - Y-coordinate in the world coordinate system in mm as INT # @param alpha Zielwinkel relativ zum Weltkoordinatensystem in rad als FLOAT - Target angle in the world coordinate system in rad as FLOAT # @return True, wenn ein Pfad geplant wurde und die Plattform fährt - True if planning was successful and movement has started # @return False im Fehlerfall - False if an error occured # # Schickt den Befehl, die angegebenen Zielposition in der Karte anzufahren. Die Funktion prüft, ob die Planung erfolgreich war und die Plattform fährt # (Wartezeit maximal 15s). - # Sends the command to move to the given position in the map. The function checks if planning was successful and movement has started (timeout is 15 seconds) def moveAbsoluteNonBlocking(self, x,y,alpha): self.__moveAbs(x,y,alpha) self.sendCmd(EnMsgPltfCtrl.CMD_START) timeout = 15 while self.statePltf.pltfData.pltfTargetReached: sleep(0.5) timeout = timeout -1 if (timeout < 0): return False return True ## Fährt eine Zielposition in der Karte an (blockierend) - Move to a position in the map (blocking) # @param x X-Koordinate im Weltkoordinatensystem in Millimetern als INT - X-coordinate in the world coordinate system in mm as INT # @param y Y-Koordinate im Weltkoordinatensystem in Millimetern als INT - Y-coordinate in the world coordinate system in mm as INT # @param alpha Zielwinkel relativ zum Weltkoordinatensystem in rad als FLOAT - Target angle in the world coordinate system in rad as FLOAT # # Schickt den Befehl, die angegebenen Zielposition in der Karte anzufahren. Die Funktion blockiert, bis das Ziel erreicht wurde oder die zulässige Zeit # überschritten wurde. - # Sends the command to move to an absolute position in the map. Function blocks until the target has been reached without timeout. def moveAbsoluteBlocking(self, x,y,alpha): self.__moveAbs(x,y,alpha) self.sendCmd(EnMsgPltfCtrl.CMD_START) while self.statePltf.pltfData.pltfTargetReached: sleep(0.5) while not self.statePltf.pltfData.pltfTargetReached: sleep(0.5) print str(ctime()) + " position reached." def __moveAbs(self, x, y, alpha): msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_MOVE_ABS) msg.writeInt(x) msg.writeInt(y) msg.writeInt(PltfCtrlClient.convertRadToDegree(alpha)) msg.setHasReply(False) msg.setMessageType(EnMsgPltfCtrl.CMD_MOVE_ABS) self.socketClient.addToSendQueue(msg) ## Fährt eine Zielposition im Plattformkoordinatensystem an (nicht blockierend) - Move to a position in the platform's coordinate system (non blocking) # @param x X-Koordinate im Plattformkoordinatensystem in Millimetern als INT - X-coordinate in the platform coordinate system in mm as INT # @param y Y-Koordinate im Plattformkoordinatensystem in Millimetern als INT - Y-coordinate in the platform coordinate system in mm as INT # @param alpha Zielwinkel relativ zum Plattformkoordinatensystem in rad als FLOAT - Target angle in the platform coordinate system in rad as FLOAT # @return True, wenn ein Pfad geplant wurde und die Plattform fährt - True if planning was successful and movement has started # @return False im Fehlerfall - False if an error occured # # Schickt den Befehl, die angegebenen relative Zielposition anzufahren. Die Funktion prüft, ob die Planung erfolgreich war und die Plattform fährt # (Wartezeit maximal 15s). - # Sends the command to move to the given position relative to the platform. The function checks if planning was successful and movement has started (timeout is 15 seconds) def moveRelativeNonBlocking(self, x,y,alpha): self.__moveRel(x,y,alpha) self.sendCmd(EnMsgPltfCtrl.CMD_START) timeout = 15 while self.statePltf.pltfData.pltfTargetReached: sleep(0.5) timeout = timeout -1 if (timeout < 0): return False return True ## Fährt eine Zielposition im Plattformkoordinatensystem an (blockierend) - Move to a position in the platform's coordinate system (blocking) # @param x X-Koordinate im Plattformkoordinatensystem in Millimetern als INT - X-coordinate in the platform's coordinate system in mm as INT # @param y Y-Koordinate im Plattformkoordinatensystem in Millimetern als INT - Y-coordinate in the platform's coordinate system in mm as INT # @param alpha Zielwinkel relativ zum Plattformkoordinatensystem in rad als FLOAT - Target angle in the platform's coordinate system in rad as FLOAT # # Schickt den Befehl, die angegebenen relative Zielposition anzufahren. Die Funktion blockiert, bis das Ziel erreicht wurde oder die zulässige Zeit # überschritten wurde. - # Sends the command to move to the given position relative to the platform. Function blocks until the target has been reached without timeout. def moveRelativeBlocking(self, x,y,alpha): self.__moveRel(x,y,alpha) self.sendCmd(EnMsgPltfCtrl.CMD_START) while self.statePltf.pltfData.pltfTargetReached: sleep(0.5) while not self.statePltf.pltfData.pltfTargetReached: sleep(0.5) print str(ctime()) + " position reached." def __moveRel(self, x, y, alpha): msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_MOVE_REL) msg.writeInt(x) msg.writeInt(y) msg.writeInt(PltfCtrlClient.convertRadToDegree(alpha)) msg.setHasReply(False) msg.setMessageType(EnMsgPltfCtrl.CMD_MOVE_REL) self.socketClient.addToSendQueue(msg) ## Fährt die Ladestation an - Start the automatic docking procedure # @pre Der Automatikmodus ist gewählt - Motion mode is MODE_MOTION_AUTOMATIC # # Löst den automatischen Adnockvorgang aus. Die Plattform muss zwei bis drei Meter vor der Ladestation stehen und sich im # Automatikmodus befinden. - # Starts the automatic docking. The platform must be 2-3 meters in front of a charging station and the automatic motion mode # must be set. def automaticDocking(self): self.sendCmd(EnMsgPltfCtrl.CMD_AUTOMATIC_CHARGE) ## Verlässt die Ladestation an - Leave charging station # @pre Der Automatikmodus ist gewählt - Motion mode is MODE_MOTION_AUTOMATIC # @pre Plattform ist automatisch in die Ladestation gefahren - Platform has docked automatically to charging station # # Plattform verlässt die Ladestation. Die Plattform fährt 80cm zurück und muss sich im Automatikmodus befinden. - # Leave the charging station. The platform moves back 80 cm. def leaveChargeStation(self): self.sendCmd(EnMsgPltfCtrl.CMD_LEAVE_CHARGESTATION) ## Aktiviert den Elastic-Band-Planer - Enable elastic band planning # @param enabled Aktiviert/Deaktiviert den Elastic-Band-Planungsalgorithmus - Enable/disable elastic band algorithm # # Diese Funktion aktiviert oder deaktiviert den reaktiven Planer zum Umfahren von Hindernissen. Bei deaktiviertem Planer # wartet die Plattform, bis der Pfad wieder frei ist. # This function enables or disables the reactive planning which is used for dynamic obstacle avoidance. # If disabled the platform just waits in front of any obstacle on the path. This will not affect the global planning! def setReactivePlanningEnabled(self, enabled): if enabled: self.setParameter(PltfParams.PlanningEnabled, 'true') else: self.setParameter(PltfParams.PlanningEnabled, 'false') ## Aktiviert die Rückwärtsfahrt - Enables the platform to move backwards # # Gibt die Möglichkeit zur Rückwärtsfahrt frei, wenn das Umdrehen der Plattform problematisch ist. - # Enables the robot to move backwards in order to avoid turning the mobile platform around. def enableBackwardMove(self): self.setParameter(PltfParams.MotionBackwardEnabled, 'true') ## Deaktiviert die Rückwärtsfahrt - Disables the possibility of the platform to move backwards # # Sperrt die Möglichkeit zur Rückwärtsfahrt. - Disables backward interpolation, the platform will only move forwards. def disableBackwardMove(self): self.setParameter(PltfParams.MotionBackwardEnabled, 'false') ## Setzt die Position der virtuellen Plattform in der Umgebungskarte - Sets the virtual platform to a new position in the map # @param x X-Koordinate der neuen Position in Millimetern als INT - The X-coordinate of the new position in mm as INT # @param y Y-Koordinate der neuen Position in Millimetern als INT - The Y-coordinate of the new position in mm as INT # @param angleDeg Der Winkel der neuen Position in Grad als INT - The new angle in degree as INT # # Mit dieser Funktion kann die Position der virtuellen Plattform innerhalb der Umgebungskarte geändert werden. - # This function changes the position of the virtual platform in the map. Useful for startup initialisation. def setPltfFrame(self, x,y, angleDeg): msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_SET_FRM_PLTF) msg.writeInt(x) msg.writeInt(y) msg.writeInt(angleDeg *10) msg.setHasReply(False) msg.setMessageType(EnMsgPltfCtrl.CMD_SET_FRM_PLTF) self.socketClient.addToSendQueue(msg) ## Liefert eine Komponente der berechneten Position der Plattform - Get one element of the estimated platform position # @param numCoord Auswahl der Positionskomponente (X=0, Y=1, Winkel=2) - Indicates the element of the pose (X=0, Y=1, Angle=2) # @return Komponente der berechneten Position - One element of the estimated pose # # Funktion zum Auslesen der aktuell vermuteten Position - Function to check the currently estimated position. def getPltfEstPos(self, numCoord): if(numCoord == 0): return self.statePltf.frmPltfEst.x elif(numCoord == 1): return self.statePltf.frmPltfEst.y else: return self.statePltf.frmPltfEst.angDeg ## Prüft ob die Pfadplanung abgeschlossen wurde - Check if path planning is finished # @return True, wenn die Planung abgeschlossen wurde - True if planning is finished # @return False, wenn die Planung noch läuft - False if planning is still in progress # # Stellt fest, ob die Plattform das zuletzt angegebenen Ziel bereits anfährt oder noch den Pfad plant. - # Planning is still in progress if a motion command has been sent and the platform is not moving. def isPlanningFinished(self): return not self.statePltf.pltfData.pltfTargetReached ## Prüft, ob die Plattform das angegebene Ziel bereits erreicht hat - Checks if the platform has already reached the target # @return True, wenn das Ziel erreicht wurde - True if the target has been reached # @return False, wenn das Ziel noch nicht erreicht wurde - False if the target has not yet been reached # # Prüft, ob die Plattform das angegebene Ziel bereits erreicht hat - Checks if the platform has already reached the target def isTargetReached(self): return self.statePltf.pltfData.pltfTargetReached ## Fragt den Zustand der Not-Halt-Tasten ab - Returns state of the emergency stop buttons # @return True bei gedrückter Not-Halt-Taste - True if emergency stop button has been pressed # @return False, wenn kein Not-Halt anliegt - False if there is no stop # # Die möglichen Zustände des Sicherheitssystems sind wie folgt kodiert - # The different cases for an emergency stop are bit coded: # Byte 4 nicht verwendet - unused\n # Byte 3 Fehler im Scanner - error of a laser scanner\n # Byte 2 Fehler auf dem RelayBoard - error on the RelayBoard\n # Byte 1 (low Byte) Halt durch scanner / Taste - stop caused by scanner / button def isEmStopActive(self): if self.statePltf.pltfData.stateEmergency == 2: return True else: return False ## Fragt den Zustand der Schutzfelder ab - Returns state of the safety field # @return True, wenn ein Hindernis im Schutzfeld erkannt wurde - True if an obstacle is in the safety field of the scanner # @return False, wenn das Schutzfeld frei ist - False if the safety field is clear # # Die möglichen Zustände des Sicherheitssystems sind wie folgt kodiert - # The different cases for an emergency stop are bit coded: # Byte 4 nicht verwendet - unused\n # Byte 3 Fehler im Scanner - error of a laser scanner\n # Byte 2 Fehler auf dem RelayBoard - error on the RelayBoard\n # Byte 1 (low Byte) Halt durch scanner / Taste - stop caused by scanner / button def isScannerStopActive(self): if self.statePltf.pltfData.stateEmergency == 1: return True else: return False ## Prüft auf Not-Halt - Checks for emergency stop # @return True, wenn kein Not-Halt vorliegt - True if there is no emergency stop # @return False, wenn ein Not-Halt vorliegt - False if the platform is stopped # def isEmergencyReset(self): if self.statePltf.pltfData.stateEmergency == 0: return True else: return False ## Prüft, ob die PLattform angehalten wurde - Checks if the platform has been stopped # @return True, wenn die Plattform angehalten wurde - True if the platform has been stopped # @return False, wenn die Plattform nicht gestoppt wurde - False if the platform has not been stopped # # Liefert den Betriebszustand von PlatformCtrl zurück. - Returns the operation state of PlatformCtrl. def isStopped(self): if self.statePltf.pltfData.stateOperation == 0: return True else: return False ## Prüft, ob die Batteriespannung unter dem angegebenen Wert liegt - Checks if the main battery voltage is below the given value # @param value Der Vergleichswert - The value to compare # @return True, wenn die Batteriespannung niedriger ist - True if current voltage is below the value # @return False, wenn die Batteriespannung nicht unter dem Vergleichswert liegt - False if the current voltage is not below the value # # Bei 24V-Systemen sollte ab 22V reagiert werden, bei 48V-Systemen ab 44V. - For 24V systems 22V is the minimum, # for 48V systems 44V is the minimum. def isVoltageUnder(self, value): if self.statePltf.boardData.relBoardAnalogIn[1] < value: return True else: return False ## Gibt die aktuelle Batteriespannung auf der Konsole aus - Print current main battery voltage to console # # Für Debuggingzwecke kann die Ausgabe der aktuellen Batteriespannung sinnvoll sein. - For debugging issues it may be necessary to print the current voltage. def printVoltage(self): print self.statePltf.boardData.relBoardAnalogIn[1] ## Fragt die aktuelle Batteriespannung ab - Get the current main battery voltage # @return Batteriespannung - main battery voltage # # Liest die aktuelle Batteriespannung aus. - Requests the main battery voltage reported by PlatformCtrl. def getVoltage(self): return self.statePltf.boardData.relBoardAnalogIn[1] ## Lädt eine Umgebungskarte auf die Plattform - Loads a map onto the platform # @param path Pfad und Name der Kartendatei - The full file name of the map to be loaded # # Lädt eine lokal gespeicherte Karte und überträgt sie auf die Plattform. - # Loads a map from a local file and sends it to the platform. def loadMap(self, path): map = Map2D() map.loadFromDisk(path) msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_SET_MAP) msg.writeString(map.name) msg.writeInt(len(map.rawData)) msg.extend(map.rawData) msg.setMessageType(EnMsgPltfCtrl.CMD_SET_MAP) msg.setHasReply(False) self.socketClient.addToSendQueue(msg) self.sendCmd(EnMsgPltfCtrl.CMD_SAVE_MAP) ## Lädt eine Roadmap auf die Plattform - Loads a roadmap onto the platform # @param path Pfad und Name der Roadmap-Datei - The full file name of the roadmap to be loaded # # Lädt eine lokal gespeicherte Roadmap und überträgt sie auf die Plattform. - # Loads a roadmap from a local file and sends it to the platform. def loadRoadMap(self, path): map = RoadMap() map.loadFromDisk(path) msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_SET_ROADMAP) msg.writeString(map.name) msg.writeInt(len(map.rawData)) msg.extend(map.rawData) msg.setMessageType(EnMsgPltfCtrl.CMD_SET_ROADMAP) msg.setHasReply(False) self.socketClient.addToSendQueue(msg) self.sendCmd(EnMsgPltfCtrl.CMD_SAVE_ROADMAP) #------------------- # Überwachungsmodus - Surveillance #------------------- ## Startet den Überwachungsmodus während der Fahrt - Starts the surveillance mode while moving # @param bOnOff Aktiviert/deaktiviert den Überwachungsmodus (BOOL) - Switches surveillance mode on / off (BOOL) # @pre Passivinfratrotsensoren sind verfügbar - Passive infrared sensors are available # # Startet die Personen- und Bewegungsdetektion per Infrarotdetektoren während der Fahrt. - Starts person detection/movement detection with PIR sensors. def enableSurveillanceModeInMovement(self, bOnOff): if bOnOff: self.__setSurveillanceParams(True, False, False, True) else: self.__setSurveillanceParams(False, False, False, False) ## Startet den Überwachungsmodus im Stillstand - Starts the surveillance mode while standing # @param bOnOff Aktiviert/deaktiviert den Überwachungsmodus (BOOL) - Switches surveillance mode on / off (BOOL) # @pre Passivinfratrotsensoren sind verfügbar - Passive infrared sensors are available # # Startet die Personen- und Bewegungsdetektion per Infrarotdetektoren und Laserscanner im Stillstand. - Starts person detection/movement detection # with PIR sensors and laser scanner. def enableSurveillanceModeStanding(self, bOnOff): if bOnOff: self.__setSurveillanceParams(True, True, False, True) else: self.__setSurveillanceParams(False, False,False, False) def __setSurveillanceParams(self, surveyEn, scannerEn, RadarEn, pirEn): self.setParameter(PltfParams.SurveillanceModeEnabled, surveyEn) self.setParameter(PltfParams.ScannerSurveillance, scannerEn) self.setParameter(PltfParams.RadarSensorsEnabled, RadarEn) self.setParameter(PltfParams.PIRSensorsEnabled, pirEn) ## Prüft, ob eine Bewegung oder Person erkannt wurde - Checks if a movement or person has been detected # @return True, wenn Alarm ausgelöst wurde - True if alarm was raised # @return False, wenn kein Alarm vorliegt - False if no alarm occured # # def isAlarm(self): return self.statePltf.survData.alarmEnabled ## Fragt den Status des Tastenfelds ab - Check the state of the built-in keypad # @param Key Bezeichnung der abzufragenden Taste als STRING - Name of the key to check as STRING # @return True, wenn die Taste gedrückt ist - True if key is pressed # @return False, wenn die Taste unbetätigt ist - False if key not pressed # # Mögliche Tasten sind / Available keys are: F1, F2, F3, F4 def isKeyPressed(self, Key): if (Key == "F1"): return self.__isKeyPadPressed(1) if (Key == "F2"): return self.__isKeyPadPressed(2) if (Key == "F3"): return self.__isKeyPadPressed(3) if (Key == "F4"): return self.__isKeyPadPressed(0) else: return False def __isKeyPadPressed(self, Key): keyPadData = self.statePltf.boardData.relBoardAnalogIn[3] mask = 1 << Key if (keyPadData & mask) == 0: return True else: return False ## Wartet, bis eine Taste gedrückt wird - Wait until any key is pressed # # Funktion blockiert bis eine beliebige Taste gedrückt wird. - # Function blocks until one key is pressed on the built-in keypad. def waitUntilKeyPressed(self): while 1: if (self.__isKeyPadPressed(1)): return "F1" if (self.__isKeyPadPressed(2)): return "F2" if (self.__isKeyPadPressed(3)): return "F3" if (self.__isKeyPadPressed(0)): return "F4" sleep(0.3) ## Stellt fest, welche Taste momentan gedrückt wird - Get the currently pressed key # @return Bezeichnung der gedrückten Taste als STRING - Key as STRING # # Liefert die momentan gedrückte Taste zurück. Werden mehrere Tasten gleichzeitig gedrückt, wird nur die Taste mit # dem niedrigsten Index zurückgegeben. - # Get the current state of the built-in keypad. If more than one key is pressed only the first one will be returned. def getPressedKey(self): if (self.__isKeyPadPressed(1)): return "F1" if (self.__isKeyPadPressed(2)): return "F2" if (self.__isKeyPadPressed(3)): return "F3" if (self.__isKeyPadPressed(0)): return "F4" else: return "None" def setDigOut(self, channel, value, boardType): msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_SET_DIGOUT) msg.writeInt(channel) msg.writeBoolean(value) msg.writeInt(boardType) msg.setMessageType(EnMsgPltfCtrl.CMD_SET_DIGOUT) msg.setHasReply(False) self.socketClient.addToSendQueue(msg) ## Schaltet alle digitalen Ausgänge des IOBoards ab - Switches all digital outputs of the IOBoard to off # @pre Ein IOBoard ist verfügbar - An IOBoard is available # # Funktion zum einfachen Abschalten aller Digitalausgänge des IOBoards. - # Function to conveniently switch off all digitial outputs of the IOBoard. def switchAllIOBoardChannelsOff(self): for channel in range(1,16): self.sendSetDigOut(channel, False, BoardType.TYPE_IOBOARD) sleep(0.2) ## Schaltet einen digitalen Ausgang des IOBoards an - Enables one digital output of the IOBoard # @param channel Der zu schaltende Ausgang - The channel to be switched # @pre Ein IOBoard ist verfügbar - An IOBoard is available # # Schaltet einen digitalen Ausgang des IOBoards an - Enables one digital output of the IOBoard def enableIOBoardChannel(self, channel): self.sendSetDigOut(channel, True, BoardType.TYPE_IOBOARD) sleep(0.2) ## Schaltet einen digitalen Ausgang des IOBoards aus - Disables one digital output of the IOBoard # @param channel Der zu schaltende Ausgang - The channel to be switched # @pre Ein IOBoard ist verfügbar - An IOBoard is available # # Schaltet einen digitalen Ausgang des IOBoards aus - Disables one digital output of the IOBoard def disableIOBoardChannel(self, channel): self.sendSetDigOut(channel, False, BoardType.TYPE_IOBOARD) sleep(0.2) ####################### # Relay Board Control # ####################### ## Schaltet einen digitalen Ausgang des RelayBoards an - Enables a digital output on the RelayBoard # @param iChannel Der zu schaltende Ausgang - The channel to be switched # # Schaltet einen digitalen Ausgang des RelayBoards an - Enables one digital output of the RelayBoard def enableRelayChannel(self, iChannel): self.sendSetDigOut(iChannel, True, BoardType.TYPE_RELAYBOARD) sleep(0.2) ## Schaltet einen digitalen Ausgang des RelayBoards aus - Disables a digital output on the RelayBoard # @param iChannel Der zu schaltende Ausgang - The channel to be switched # # Schaltet einen digitalen Ausgang des RelayBoards aus - Disables one digital output of the RelayBoard def disableRelayChannel(self, iChannel): self.sendSetDigOut(iChannel, False, BoardType.TYPE_RELAYBOARD) sleep(0.2) ###################### # Additional Control # ###################### def __writeLCD(self, text, modeMotion): msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_WRITE_LCD) msg.writeBoolean(modeMotion) msg.writeString(text) msg.setHasReply(False) msg.setMessageType(EnMsgPltfCtrl.CMD_WRITE_LCD) self.socketClient.addToSendQueue(msg) ## Schreibt einen Text auf das LCD - Write a text to the LCD # @param text Text als STRING - Text as STRING # # Die letzte Zeile des vierzeiligen LCD kann mit beliebiegen Nachrichten (bis 20 Zeichen) beschrieben werden. - # This function prints a message (up to 20 characters) to the last line of the LCD. def writeLCD(self, text): self.__writeLCD(text, False) ## schreibt den Bewegungsmodus auf das LCD - Writes the motion mode to the LCD # @param text Modusbezeichnung als STRING - Mode as STRING # # Diese Funktion füllt auf dem LCD die Position nach "MODE" mit Text. - This function sets the text after the header "MODE". def writeModeMotionToLCD(self, text): self.__writeLCD(text, True) ## Löscht die letzte Zeile des LCD - Clears the last line of the LCD # # Löscht die letzte Zeile des eingebauten LCD. - Clears the last line of the built-in LCD. def clearLCD(self): self.__writeLCD("", False) ## Gibt den Breitengrad aus der GPS-Messung zurück - Get the GPS latitude # @return Breitengrad - Latitude # @pre GPS-Modul ist verfügbar und hat die Position bestimmt - GPS module is available and has found the current position # # def getGPSLat(self): return self.statePltf.boardData.gpsLat ## Gibt den Längengrad aus der GPS-Messung zurück - Get the GPS longitude # @return Längengrad - Longitude # @pre GPS-Modul ist verfügbar und hat die Position bestimmt - GPS module is available and has found the current position # # def getGPSLon(self): return self.statePltf.boardData.gpsLon #------------------- # Additional Functions #------------------- ## Gibt einen Text über die Text-to-speech Software aus - Speack text using text-to-speech software # @param Text als STRING - Text as STRING # @pre Text-to-speech Software ist verfügbar - Text-to-speech software is available # @pre Audiosystem ist installiert - Audio system is available # # Mit dieser Funktion wird der übergebene Text per Text-to-speech Software wiedergegeben. - # This function uses a text-to-speech system to read the given string. def speak(self, text): msg = Message() msg.writeByte(EnMsgPltfCtrl.CMD_SPEAK_TEXT) msg.writeString(text) msg.setMessageType(EnMsgPltfCtrl.CMD_SPEAK_TEXT) msg.setHasReply(False) self.socketClient.addToSendQueue(msg) @staticmethod def convertRadToDegree(angRad): return angRad * 57.29577951308232 if __name__ == "__main__": pltfClient = PltfCtrlClient() for i in range(1): pltfClient.connect('127.0.0.1') sleep(3) pltfClient.loadMap("C:/Users/Trackium/Desktop/MobileArmGUI - JH/content/map/Neobotix.map") pltfClient.loadRoadMap("C:/Users/Trackium/Desktop/MobileArmGUI - JH/content/roadmap/Neobotix.rm") sleep(3) pltfClient.disconnect() sleep(2)