Source code for OpNavMonteCarlo

#
#  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.
#
r"""
Overview
--------

The OpNav Monte-Carlo python scripts provides the capability to generate images and truth data in order to
train neural networks for image processing.

This script calls OpNavScenarios/CNN_ImageGen/scenario_CNNImages.py in order to generate the simulations.
The script can be called by running::

    python3 OpNavMonteCarlo.py

"""



import csv
import inspect
import os

import scenario_CNNImages as scenario

filename = inspect.getframeinfo(inspect.currentframe()).filename
path = os.path.dirname(os.path.abspath(filename))

from Basilisk import __path__
bskPath = __path__[0]

from Basilisk.utilities.MonteCarlo.Controller import Controller, RetentionPolicy
from Basilisk.utilities.MonteCarlo.Dispersions import OrbitalElementDispersion, MRPDispersionPerAxis, UniformDispersion

# import simulation related support
from Basilisk.utilities import RigidBodyKinematics as rbk
from Basilisk.utilities import unitTestSupport
from Basilisk.utilities import macros
import matplotlib.pyplot as plt
import numpy as np


retainedMessageName1 = "scMsg"
retainedMessageName2 = "circlesMsg"
retainedRate = macros.sec2nano(10)
var1 = "r_BN_N"
var2 = "sigma_BN"
var3 = "valid"

[docs]def run(show_plots): """Main Simulation Method""" NUMBER_OF_RUNS = 10 VERBOSE = True PROCESSES = 1 RUN = True POST = True dirName = os.path.abspath(os.path.dirname(__file__)) + "/cnn_MC_data" if RUN: myExecutionFunction = scenario.run myCreationFunction = scenario.scenario_OpNav monteCarlo = Controller() monteCarlo.setShouldDisperseSeeds(True) monteCarlo.setExecutionFunction(myExecutionFunction) monteCarlo.setSimulationFunction(myCreationFunction) monteCarlo.setExecutionCount(NUMBER_OF_RUNS) monteCarlo.setThreadCount(PROCESSES) monteCarlo.setVerbose(True) monteCarlo.setArchiveDir(dirName) # Add some dispersions dispDict = {} dispDict["mu"] = 4.2828371901284001E+13 dispDict["a"] = ["normal", 14000*1E3, 2500*1E3] # 12000 dispDict["e"] = ["uniform", 0.2, 0.5] # 0.4, 0.7 dispDict["i"] = ["uniform", np.deg2rad(40), np.deg2rad(90)] dispDict["Omega"] = None dispDict["omega"] = None dispDict["f"] = ["uniform", np.deg2rad(0), np.deg2rad(359)] disp1Name = 'get_DynModel().scObject.hub.r_CN_NInit' disp2Name = 'get_DynModel().scObject.hub.v_CN_NInit' disp3Name = 'get_FswModel().trackingErrorCam.sigma_R0R' dispGauss = 'get_DynModel().cameraMod.gaussian' dispDC = 'get_DynModel().cameraMod.darkCurrent' dispSP = 'get_DynModel().cameraMod.saltPepper' dispCR = 'get_DynModel().cameraMod.cosmicRays' dispBlur = 'get_DynModel().cameraMod.blurParam' monteCarlo.addDispersion(OrbitalElementDispersion(disp1Name,disp2Name, dispDict)) monteCarlo.addDispersion(MRPDispersionPerAxis(disp3Name, bounds=[[1./3-0.051, 1./3+0.051], [1./3-0.051, 1./3+0.051], [-1./3-0.051, -1./3+0.051]])) monteCarlo.addDispersion(UniformDispersion(dispGauss, [0,5])) monteCarlo.addDispersion(UniformDispersion(dispSP, [0,2.5])) monteCarlo.addDispersion(UniformDispersion(dispCR, [0.5,4])) monteCarlo.addDispersion(UniformDispersion(dispBlur, [1,6])) # Add retention policy retentionPolicy = RetentionPolicy() retentionPolicy.addMessageLog(retainedMessageName1, [var1, var2]) retentionPolicy.addMessageLog(retainedMessageName2, [var3]) monteCarlo.addRetentionPolicy(retentionPolicy) failures = monteCarlo.executeSimulations() assert len(failures) == 0, "No runs should fail" if POST: monteCarlo = Controller.load(dirName) for i in range(0,NUMBER_OF_RUNS): try: monteCarloData = monteCarlo.getRetainedData(i) except FileNotFoundError: print("File not found, ", i) continue csvfile = open(dirName + "/run" + str(i) + "/data.csv", 'w') writer = csv.writer(csvfile) writer.writerow(['Filename', 'Valid', 'X_p', 'Y_p', 'rho_p', 'r_BN_N_1', 'r_BN_N_2', 'r_BN_N_3']) timeAxis = monteCarloData["messages"][retainedMessageName1 + ".times"] position_N = unitTestSupport.addTimeColumn(timeAxis, monteCarloData["messages"][retainedMessageName1 + "." + var1]) sigma_BN = unitTestSupport.addTimeColumn(timeAxis, monteCarloData["messages"][retainedMessageName1 + "." + var2]) validCircle = unitTestSupport.addTimeColumn(timeAxis, monteCarloData["messages"][retainedMessageName2 + "." + var3]) renderRate = 60*1E9 sigma_CB = [0., 0., 0.] # Arbitrary camera orientation sizeOfCam = [512, 512] sizeMM = [10. * 1E-3, 10. * 1E-3] # in m fieldOfView = np.deg2rad(55) # in degrees focal = sizeMM[0] / 2. / np.tan(fieldOfView / 2.) # in m pixelSize = [] pixelSize.append(sizeMM[0] / sizeOfCam[0]) pixelSize.append(sizeMM[1] / sizeOfCam[1]) dcm_CB = rbk.MRP2C(sigma_CB) # Plot results trueRhat_C = np.full([len(validCircle[:, 0]), 4], np.nan) trueCircles = np.full([len(validCircle[:, 0]), 4], np.nan) trueCircles[:, 0] = validCircle[:, 0] trueRhat_C[:, 0] = validCircle[:, 0] ModeIdx = 0 Rmars = 3396.19 * 1E3 for j in range(len(position_N[:, 0])): if position_N[j, 0] in validCircle[:, 0]: ModeIdx = j break for i in range(len(validCircle[:, 0])): if validCircle[i, 1] > 1E-5 or (validCircle[i, 0]%renderRate ==0 and validCircle[i, 0] > 1): trueRhat_C[i, 1:] = np.dot(np.dot(dcm_CB, rbk.MRP2C(sigma_BN[ModeIdx + i, 1:4])), position_N[ModeIdx + i, 1:4]) / np.linalg.norm(position_N[ModeIdx + i, 1:4]) trueCircles[i, 3] = focal * np.tan(np.arcsin(Rmars / np.linalg.norm(position_N[ModeIdx + i, 1:4]))) / pixelSize[0] trueRhat_C[i, 1:] *= focal / trueRhat_C[i, 3] trueCircles[i, 1] = trueRhat_C[i, 1] / pixelSize[0] + sizeOfCam[0] / 2 - 0.5 trueCircles[i, 2] = trueRhat_C[i, 2] / pixelSize[1] + sizeOfCam[1] / 2 - 0.5 writer.writerow([str("{0:.6f}".format(position_N[i,0]*1E-9))+".jpg", validCircle[i, 1], trueCircles[i, 1], trueCircles[i, 2], trueCircles[i, 3], position_N[i,1], position_N[i,2], position_N[i,3]]) csvfile.close() if show_plots: monteCarlo.executeCallbacks() plt.show() return
if __name__ == "__main__": pid = run(True)