## @package StatePltf # Data Structure for a Platform Status. Updated automatically by its holder, PltfCtrlClient. # # ----------------------------------------------------------------------------------- # # Copyright (c) 2009 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 # # ----------------------------------------------------------------------------------- class StatePltf(object): def __init__(self): self.frmPltfCmd = Frame2D() self.frmPltfEst = Frame2D() self.covPltfEst = CovarianceData() self.cmdVelTransMMS = 0 self.cmdVelRotRadS = 0 self.pathIpo = [] self.boardData = BoardData() self.pltfData = PlatformData() self.survData = SurveillanceData() def updateStateFromMessage(self, msg): # Command cmd = msg.readByte() # Frame Platform Command self.frmPltfCmd.x = msg.readInt() self.frmPltfCmd.y = msg.readInt() self.frmPltfCmd.angDeg = msg.readInt() / 1000.0 # Frame Platform Estimated self.frmPltfEst.x = msg.readInt() self.frmPltfEst.y = msg.readInt() self.frmPltfEst.angDeg = msg.readInt() / 1000.0 # Platform covariance self.covPltfEst.covX = msg.readInt() self.covPltfEst.covY = msg.readInt() self.covPltfEst.covADeg = msg.readInt() / 1000.0 self.covPltfEst.angToXDeg = msg.readInt() / 1000.0 # Scanner Data - SKIPPING size = msg.readInt() for i in range(size): msg.readShortInt() msg.readShortInt() # Reflectors - SKIPPING size = msg.readInt() for i in range(size): msg.readInt() msg.readInt() # Lines - SKIPPING size = msg.readInt() for i in range(size): msg.readInt() msg.readInt() msg.readInt() msg.readInt() # Circles - SKIPPING size = msg.readInt() for i in range(size): msg.readInt() msg.readInt() msg.readInt() # Corners - SKIPPING size = msg.readInt() for i in range(size): msg.readInt() msg.readInt() # Interpolated Path size = msg.readInt() self.pathIpo = [] for i in range(size): frm = Frame2D() frm.x = msg.readInt() frm.y = msg.readInt() self.pathIpo.append(frm) # Joystick Buttons - SKIPPING size = msg.readInt() for i in range(size): msg.readBoolean() # Velocities Command self.cmdVelTransMMS = msg.readInt() self.cmdVelRotRadS = msg.readInt() / 1000.0 # RelayBoard self.boardData.relBoardDigIn = msg.readInt() self.boardData.relBoardState = msg.readInt() for i in range(8): self.boardData.relBoardAnalogIn[i] = msg.readInt() for i in range(4): self.boardData.optoSensorDist[i] = msg.readInt() # IOBoard self.boardData.ioBoardBattVoltage = msg.readInt() self.boardData.ioBoardDigIn = msg.readInt() self.boardData.ioBoardDigOut = msg.readInt() self.boardData.ioBoardState = msg.readInt() for i in range(8): self.boardData.ioBoardAnalogIn[i] = msg.readInt() # MotorCtrl Boards self.boardData.motCtrlBoardRightStatus = msg.readInt() self.boardData.motCtrlBoardLeftStatus = msg.readInt() self.boardData.motCtrlBoardRightTemp = msg.readInt() self.boardData.motCtrlBoardLeftTemp = msg.readInt() # USBoard distances for i in range(16): self.boardData.usBoardSensorDist[i] = msg.readInt() # USBoard analog in for i in range(8): self.boardData.usBoardAnalogIn[i] = msg.readInt() self.boardData.usBoardState = msg.readInt() # RadarBoards 1-4 for i in range(4): self.boardData.radarBoardVelMMS[i] = msg.readInt() self.boardData.radarBoardState = msg.readInt() # GPS Data self.boardData.gpsLat = msg.readDouble() self.boardData.gpsLon = msg.readDouble() # GyroBoard self.boardData.gyroBoardAccSens[0] = msg.readInt() self.boardData.gyroBoardAccSens[1] = msg.readInt() self.boardData.gyroBoardAccSens[2] = msg.readInt() self.boardData.gyroBoardAngRad = msg.readInt() self.boardData.gyroBoardTemp = msg.readInt() self.boardData.gyroBoardState = msg.readInt() # Misc self.pltfData.pltfInit = msg.readBoolean() self.pltfData.pltfTargetReached = msg.readBoolean() self.pltfData.motionMode = msg.readByte() self.pltfData.stateOperation = msg.readByte() self.pltfData.stateEmergency = msg.readInt() self.pltfData.appStateOperation = msg.readByte() self.pltfData.plannerState = msg.readInt() self.pltfData.timeStamp = msg.readInt() self.pltfData.appErr = msg.readInt() self.pltfData.dockedToChargeStation = msg.readBoolean() self.pltfData.stateLocalization = msg.readBoolean() self.pltfData.collision = msg.readBoolean() self.pltfData.isCalcLookupDatPercent = msg.readByte() # Surveillance for i in range(4): self.survData.PIRSensorActive[i] = msg.readBoolean() self.survData.alarmSirenEnabled = msg.readBoolean() self.survData.flashLightEnabled = msg.readBoolean() # Head Drive position self.pltfData.posHeadDriveDeg = msg.readInt() # Message Format Check check = msg.readInt() if check != -1: print "Bad Status Message Format" class Frame2D(object): def __init__(self): self.x = 0 self.y = 0 self.angDeg = 0 class CovarianceData(object): def __init__(self): self.covX = 0 self.covY = 0 self.covADeg = 0 self.angToXDeg = 0 class BoardData(object): def __init__(self): # RelayBoard self.relBoardDigIn = 0 self.relBoardAnalogIn = [0]*8 self.relBoardState = 0 #Distances of Optosensors self.optoSensorDist = [0]*4 # IOBoard self.ioBoardBattVoltage = 0 self.ioBoardDigIn = 0 self.ioBoardDigOut = 0 self.ioBoardAnalogIn = [0]*8 self.ioBoardState = 0 # USBoard self.usBoardSensorDist = [0]*16 self.usBoardAnalogIn = [0]*8 self.usBoardState = 0 # Motctrlboard self.motCtrlBoardRightStatus = 0 self.motCtrlBoardLeftStatus = 0 self.motCtrlBoardRightTemp = 0 self.motCtrlBoardLeftTemp = 0 # Radarboard self.radarBoardVelMMS = [0]*4 self.radarBoardState = 0 # Gyroboard self.gyroBoardAccSens = [0.]*3 self.gyroBoardTemp = 0 self.gyroBoardAngRad = 0. self.gyroBoardState = 0 #GPS self.gpsLat= 0. self.gpsLon = 0. self.gpsCalibration = 0 # cinneoE self.iCinneoEEncoderValue = [0]*5 self.iCinneoEHeight = 0 class PlatformData(object): def __init__(self): self.pltfInit = False self.pltfTargetReached = False self.motionMode = 0 self.stateOperation = 0 self.stateMotion = 0 self.appStateOperation = 0 self.appErr = 0 self.dockedToChargeStation = False self.simEnabled = False self.plannerState = 0 self.timeStamp = 0 self.collision = False self.posHeadDriveDeg = 0 # State Emergency obtained from RelaisBoard, bit coded # byte 4 free # byte 3 em error scanner # byte 2 em error relais board # byte 1 (low Byte) em scanner, em button self.stateEmergency = 0 self.stateLocalization = False self.isCalcLookupDatPercent = 0 self.chargePlugConnected = False class SurveillanceData(object): def __init__(self): self.PIRSensorActive= [False]*4 self.alarmEnabled = False self.alarmSirenEnabled = False self.flashLightEnabled = False