Source code for test_dataFileToViz

#
#  ISC License
#
#  Copyright (c) 2016, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
#  Permission to use, copy, modify, and/or distribute this software for any
#  purpose with or without fee is hereby granted, provided that the above
#  copyright notice and this permission notice appear in all copies.
#
#  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
#  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
#  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
#  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
#  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
#  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
#  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
#


#
#   Unit Test Script
#   Module Name:        vizInterfaceDataFile
#   Author:             Hanspeter Schaub
#   Creation Date:      May 12, 2020
#


import os

import numpy as np
import pytest
from Basilisk.architecture import bskLogging
from Basilisk.architecture import messaging
from Basilisk.simulation import dataFileToViz
from Basilisk.simulation import spacecraft
from Basilisk.utilities import RigidBodyKinematics as rbk
# Import all of the modules that we are going to be called in this simulation
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import macros
from Basilisk.utilities import simIncludeGravBody
from Basilisk.utilities import unitTestSupport
from Basilisk.utilities import vizSupport

try:
    from Basilisk.simulation import vizInterface
except ImportError:
    pass

path = os.path.dirname(os.path.abspath(__file__))

dataFileName = None
[docs] @pytest.mark.parametrize("convertPosUnits", [-1, 1000]) @pytest.mark.parametrize("attType", [-1, 0, 1, 2]) @pytest.mark.parametrize("checkThruster", [False, True]) @pytest.mark.parametrize("checkRW", [False, True]) def test_module(show_plots, convertPosUnits, attType, checkThruster, checkRW): """ **Validation Test Description** This section describes the specific unit tests conducted on this module. The test reads in simulation from ``data.txt``, run the module, and compares the Basilisk spacecraft state messages with known values. Args: convertPosUnits (double): If positive, then this conversion factor is set. If negative, then the default value of 1000. is checked. attType (int): -1 (use default), 0 (MRP), 1 (quaternion), 2 (3-2-1 Euler Angles) checkThruster (bool): flag to check for simulation data with thrusters checkRW (bool): flag to check for simulation data with RW information **Description of Variables Being Tested** In this file, we are checking the values of the spacecraft state output message for both spacecraft: - ``r_BN_N[3]`` - ``sigma_BN[3]`` - ``thrustForce`` which is pulled from the log data to see if they match with the expected truth values. """ # each test method requires a single assert method to be called [testResults, testMessage] = run(show_plots, convertPosUnits, attType, checkThruster, checkRW, False) assert testResults < 1, testMessage global dataFileName if os.path.exists(dataFileName): os.remove(dataFileName)
def run(show_plots, convertPosUnits, attType, checkThruster, checkRW, verbose): if not verbose: bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING) testFailCount = 0 # zero unit test result counter testMessages = [] # create empty array to store test log messages unitTaskName = "unitTask" # arbitrary name (don't change) unitProcessName = "TestProcess" # arbitrary name (don't change) # Create a sim module as an empty container unitTestSim = SimulationBaseClass.SimBaseClass() # Create test thread dtSeconds = 0.1 simTimeSeconds = 2.0 testProcessRate = macros.sec2nano(dtSeconds) simulationTime = macros.sec2nano(simTimeSeconds) testProc = unitTestSim.CreateNewProcess(unitProcessName) testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) # create the simulation data file rB1N = [6761.48, 1569.01, 905.867] vB1N = [-1.95306, 6.3124, 3.64446] betaB1N = [0.182574, 0.365148, 0.547723, 0.730297] sigmaB1N = [0.1, 0.2, 0.3] omega = [0., 0., 0.] rB2N = [6761.48, 1569.02, 905.874] vB2N = [-1.95308, 6.31239, 3.64446] betaB2N = [-0.182574, 0.365148, 0.547723, 0.730297] sigmaB2N = [-0.1, 0.1, 0.3] global dataFileName dataFileName = "data" + str(convertPosUnits) + str(attType) + str(checkThruster) + str(checkRW) + ".txt" dataFileName = os.path.join(path, dataFileName) delimiter = "," fDataFile = open(dataFileName, "w+") for i in range(0, int(simTimeSeconds/dtSeconds)+2): t = round(i*dtSeconds, 4) # sc1 lineString = str(t) + delimiter + str(rB1N)[1:-1] + delimiter + str(vB1N)[1:-1] + delimiter if attType == 1: lineString += str(betaB1N)[1:-1] + delimiter else: lineString += str(sigmaB1N)[1:-1] + delimiter lineString += str(omega)[1:-1] + delimiter if checkThruster: th1ACS = 1. th1DV = 100. numACS1 = 1 numDV1 = 1 lineString += str(th1ACS) + delimiter + str(th1DV) + delimiter if checkRW: Omega1sc1 = 100.*macros.RPM u1sc1 = 0.1 Omega2sc1 = 500. * macros.RPM u2sc1 = -0.1 lineString += str(Omega1sc1) + delimiter + str(u1sc1) + delimiter lineString += str(Omega2sc1) + delimiter + str(u2sc1) + delimiter # sc2 lineString += str(rB2N)[1:-1] + delimiter + str(vB2N)[1:-1] + delimiter if attType == 1: lineString += str(betaB2N)[1:-1] + delimiter else: lineString += str(sigmaB2N)[1:-1] + delimiter lineString += str(omega)[1:-1] if checkThruster: th2ACS = 0.001 th2DV = 200. numACS2 = 1 numDV2 = 2 lineString += delimiter + str(th2ACS) + delimiter + str(th2DV) + delimiter + str(th2DV) if checkRW: Omega1sc2 = 1000.*macros.RPM u1sc2 = 0.3 lineString += delimiter + str(Omega1sc2) + delimiter + str(u1sc2) lineString += '\n' fDataFile.write(lineString) fDataFile.close() # Construct algorithm and associated C++ container testModule = dataFileToViz.DataFileToViz() testModule.ModelTag = "testModule" # set number of satellites testModule.setNumOfSatellites(2) # load the data path from the same folder where this python script is testModule.dataFileName = dataFileName testModule.delimiter = delimiter if convertPosUnits > 0: testModule.convertPosToMeters = convertPosUnits else: convertPosUnits = 1000. if attType >= 0: testModule.attitudeType = attType scNames = ["test1", "test2"] if checkThruster: # sc1 thSetAdcs1 = dataFileToViz.ThrClusterMap() thSetAdcs1.thrTag = "adcs" thSetAdcs1.color = vizSupport.toRGBA255("red") thSetDV1 = dataFileToViz.ThrClusterMap() thSetDV1.thrTag = "dv" thSetDV1.color = vizSupport.toRGBA255("blue") thList1 = [thSetAdcs1, thSetDV1] numTh1 = [1, 1] testModule.appendThrClusterMap(dataFileToViz.VizThrConfig(thList1), dataFileToViz.IntVector(numTh1)) # set ACS thruster position and direction states testModule.appendThrPos([0, 0, 3.]) # thr location in B frame, meters testModule.appendThrDir([0, 0, -1]) # thr force direction testModule.appendThrForceMax(th1ACS) # set DV thruster position and direction states testModule.appendThrPos([0., 0., -3.]) testModule.appendThrDir([0, 0, 1]) testModule.appendThrForceMax(th1DV) # sc2 thSetAdcs2 = dataFileToViz.ThrClusterMap() thSetAdcs2.thrTag = "adcs" thSetDV2 = dataFileToViz.ThrClusterMap() thSetDV2.thrTag = "dv" thList2 = [thSetAdcs2, thSetDV2] numTh2 = [1, 2] testModule.appendThrClusterMap(dataFileToViz.VizThrConfig(thList2), dataFileToViz.IntVector(numTh2)) # set ACS thruster position and direction states testModule.appendThrPos([0, 0, 3.]) testModule.appendThrDir([0, 0, -1]) testModule.appendThrForceMax(th2ACS) # set DV thruster position and direction states testModule.appendThrPos([0., 0., -3.]) testModule.appendThrDir([0, 0, 1]) testModule.appendThrForceMax(th2DV) testModule.appendThrPos([0., 2., -3.]) testModule.appendThrDir([0, 0, 1]) testModule.appendThrForceMax(th2DV) thrNumList = [numTh1, numTh2] if checkRW: # set number of RW for SC1 testModule.appendNumOfRWs(2) # RW 1 testModule.appendRwPos([0, 0, 0]) testModule.appendRwDir([1, 0, 0]) testModule.appendOmegaMax(3000.*macros.RPM) testModule.appendUMax(0.5) # RW2 testModule.appendRwPos([0, 0, 0]) testModule.appendRwDir([0, 1, 0]) testModule.appendOmegaMax(3000.*macros.RPM) testModule.appendUMax(0.5) # set number of RW for SC2 testModule.appendNumOfRWs(1) # RW 1 testModule.appendRwPos([0, 0, 0]) testModule.appendRwDir([0, 1, 0]) testModule.appendOmegaMax(3000.*macros.RPM) testModule.appendUMax(0.5) # Add module to the task unitTestSim.AddModelToTask(unitTaskName, testModule) # clear prior gravitational body and SPICE setup definitions gravFactory = simIncludeGravBody.gravBodyFactory() # setup Earth Gravity Body earth = gravFactory.createEarth() earth.isCentralBody = True # ensure this is the central gravitational body # create SC dummy objects to setup basic Vizard settings. Only one has to have the Grav Bodies attached # to show up in Vizard scObject1 = spacecraft.Spacecraft() scObject1.gravField.gravBodies = spacecraft.GravBodyVector(list(gravFactory.gravBodies.values())) scObject2 = spacecraft.Spacecraft() viz = vizSupport.enableUnityVisualization(unitTestSim, unitTaskName, [scObject1, scObject2] # , saveFile=__file__ ) if vizSupport.vizFound: # over-ride the default to not read the SC states from scObjects, but set them directly # to read from the dataFileToFiz output message viz.scData.clear() for c in range(len(scNames)): scData = vizInterface.VizSpacecraftData() scData.spacecraftName = scNames[c] scData.scStateInMsg.subscribeTo(testModule.scStateOutMsgs[c]) if checkThruster: thrList = [] thrInfo = [] for thrLogMsg in testModule.thrScOutMsgs[c]: # loop over the THR cluster log message thrList.append(thrLogMsg.addSubscriber()) k = 0 for info in testModule.thrMsgDataSC[c]: for i in range(thrNumList[c][k]): thrInfo.append(info) k += 1 scData.thrInMsgs = messaging.THROutputMsgInMsgsVector(thrList) scData.thrInfo = vizInterface.ThrClusterVector(thrInfo) if checkRW: rwList = [] for rwLogMsg in testModule.rwScOutMsgs[c]: rwList.append(rwLogMsg.addSubscriber()) scData.rwInMsgs = messaging.RWConfigLogMsgInMsgsVector(rwList) viz.scData.push_back(scData) if checkThruster: viz.settings.defaultThrusterColor = vizSupport.toRGBA255("yellow") # Setup logging on the test module output message so that we get all the writes to it dataLog = [] for scCounter in range(2): dataLog.append(testModule.scStateOutMsgs[scCounter].recorder()) unitTestSim.AddModelToTask(unitTaskName, dataLog[-1]) if checkThruster: dataThrLog = [] # SC1 for i in range(numACS1 + numDV1): dataThrLog.append(testModule.thrScOutMsgs[0][i].recorder()) unitTestSim.AddModelToTask(unitTaskName, dataThrLog[-1]) # SC2 for i in range(numACS2 + numDV2): dataThrLog.append(testModule.thrScOutMsgs[1][i].recorder()) unitTestSim.AddModelToTask(unitTaskName, dataThrLog[-1]) if checkRW: dataSc1RW1Log = testModule.rwScOutMsgs[0][0].recorder() dataSc1RW2Log = testModule.rwScOutMsgs[0][1].recorder() dataSc2RW1Log = testModule.rwScOutMsgs[1][0].recorder() unitTestSim.AddModelToTask(unitTaskName, dataSc1RW1Log) unitTestSim.AddModelToTask(unitTaskName, dataSc1RW2Log) unitTestSim.AddModelToTask(unitTaskName, dataSc2RW1Log) # Need to call the self-init and cross-init methods unitTestSim.InitializeSimulation() unitTestSim.ConfigureStopTime(simulationTime) # Begin the simulation time run set above unitTestSim.ExecuteSimulation() # This pulls the actual data log from the simulation run. pos1 = dataLog[0].r_BN_N pos2 = dataLog[1].r_BN_N att1 = dataLog[0].sigma_BN att2 = dataLog[1].sigma_BN if checkThruster: thrData = [] for item in dataThrLog: thrData.append(item.thrustForce) if checkRW: rw1Sc1OmegaData = dataSc1RW1Log.Omega rw1Sc1uData = dataSc1RW1Log.u_current rw2Sc1OmegaData = dataSc1RW2Log.Omega rw2Sc1uData = dataSc1RW2Log.u_current rw1Sc2OmegaData = dataSc2RW1Log.Omega rw1Sc2uData = dataSc2RW1Log.u_current # set input data pos1In = np.array(rB1N) pos2In = np.array(rB2N) if attType == 1: att1In = rbk.EP2MRP(np.array(betaB1N)) att2In = rbk.EP2MRP(np.array(betaB2N)) else: att1In = np.array(sigmaB1N) att2In = np.array(sigmaB2N) if attType == 2: att1In = rbk.euler3212MRP(att1In) att2In = rbk.euler3212MRP(att2In) if not unitTestSupport.isVectorEqual(pos1[0], pos1In*convertPosUnits, 0.1): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed pos1 check.") if not unitTestSupport.isVectorEqual(pos2[0], pos2In*convertPosUnits, 0.1): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed pos2 check.") if not unitTestSupport.isVectorEqual(att1[0], att1In, 0.1): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed att1 check.") if not unitTestSupport.isVectorEqual(att2[0], att2In, 0.1): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed att2 check.") if checkThruster: if not unitTestSupport.isDoubleEqualRelative(thrData[0][0], th1ACS, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed th1ACS check.") if not unitTestSupport.isDoubleEqualRelative(thrData[1][0], th1DV, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed th1ACS check.") if not unitTestSupport.isDoubleEqualRelative(thrData[2][0], th2ACS, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed th2ACS check.") if not unitTestSupport.isDoubleEqualRelative(thrData[3][0], th2DV, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed th2DV (1st) check.") if not unitTestSupport.isDoubleEqualRelative(thrData[4][0], th2DV, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed th2DV (2nd) check.") if checkRW: if not unitTestSupport.isDoubleEqualRelative(rw1Sc1OmegaData[0], Omega1sc1, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed Omega1sc1 check.") if not unitTestSupport.isDoubleEqualRelative(rw1Sc1uData[0], u1sc1, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed u1sc1 check.") if not unitTestSupport.isDoubleEqualRelative(rw2Sc1OmegaData[0], Omega2sc1, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed Omega1sc1 check.") if not unitTestSupport.isDoubleEqualRelative(rw2Sc1uData[0], u2sc1, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed u1sc1 check.") if not unitTestSupport.isDoubleEqualRelative(rw1Sc2OmegaData[0], Omega1sc2, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed Omega1sc2 check.") if not unitTestSupport.isDoubleEqualRelative(rw1Sc2uData[0], u1sc2, 0.001): testFailCount += 1 testMessages.append("FAILED: " + testModule.ModelTag + " Module failed u1sc2 check.") # print out success or failure message if testFailCount == 0: print("PASSED: " + testModule.ModelTag) else: print("Failed: " + testModule.ModelTag) print(testMessages) return [testFailCount, ''.join(testMessages)] # # This statement below ensures that the unitTestScript can be run as a # stand-along python script # if __name__ == "__main__": run( False, # showplots -1, # convertPosUnits 0, # attType (-1 -> default, 0 -> MRP, 1 -> quaternions, 2 -> 3-2-1 Euler Angles) True, # checkThruster True, # checkRW True # verbose ) if os.path.exists(dataFileName): os.remove(dataFileName)