diff --git a/src/simulation/vizard/cielimInterface/tests/test_cielimInterface.py b/src/simulation/vizard/cielimInterface/tests/test_cielimInterface.py new file mode 100644 index 000000000..375fbcc64 --- /dev/null +++ b/src/simulation/vizard/cielimInterface/tests/test_cielimInterface.py @@ -0,0 +1,220 @@ +# ISC License +# +# Copyright (c) 2024, 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. + +import tempfile, sys +from Basilisk import __path__ +bskPath = __path__[0] +sys.path.append(bskPath + "/../../src/utilities/vizProtobuffer/") +import cielimMessage_pb2 +import delimited_protobuf +import numpy as np +import pytest +from Basilisk.architecture import messaging +from Basilisk.utilities import SimulationBaseClass + +importErr = False +reasonErr = "" +try: + from Basilisk.simulation import cielimInterface +except ImportError: + importErr = True + reasonErr = "\nCielim Interface not built ---check build option" + +@pytest.mark.skipif(importErr, reason=reasonErr) +def test_interface(): + read_write_test() + +def read_write_test(): + tmpdir = tempfile.TemporaryDirectory() + unit_task_name = "unitTask" + unit_process_name = "TestProcess" + unit_test_sim = SimulationBaseClass.SimBaseClass() + + # Create test thread + test_process_rate = int(1E9) # update process rate update time + test_process = unit_test_sim.CreateNewProcess(unit_process_name) + test_process.addTask(unit_test_sim.CreateNewTask(unit_task_name, test_process_rate)) + + module = cielimInterface.CielimInterface() + module.ModelTag = "cielim_interface" + module.setOpNavMode(cielimInterface.ClosedLoopMode_OPEN_LOOP) + module.setSaveFile(tmpdir.name + "/test_proto") + module.setSaveFile("/Users/thte9300/Documents/test_proto") + + unit_test_sim.AddModelToTask(unit_task_name, module) + + # Create epoch message + epoch_payload = messaging.EpochMsgPayload() + epoch_payload.year = 2050 + epoch_payload.month = 2 + epoch_payload.day = 15 + epoch_payload.hours = 22 + epoch_payload.minutes = 8 + epoch_payload.seconds = 58 + epoch_message = messaging.EpochMsg().write(epoch_payload) + module.epochMessage.subscribeTo(epoch_message) + + # Create camera rendering message + rendering_payload = messaging.CameraRenderingMsgPayload() + rendering_payload.cameraId = 1 + rendering_payload.cosmicRayStdDeviation = 0.1 + rendering_payload.enableStrayLight = True + rendering_payload.starField = True + rendering_payload.rendering = "Lumen" + rendering_payload.smear = True + rendering_message = messaging.CameraRenderingMsg().write(rendering_payload) + module.cameraRenderingMessage.subscribeTo(rendering_message) + + # Create asteroid parameter message + asteroid_parameter_payload = messaging.CelestialBodyParametersMsgPayload() + asteroid_parameter_payload.bodyName = "asteroid_b612" + asteroid_parameter_payload.shapeModel = "Ryugu" + asteroid_parameter_payload.perlinNoise = 0.5 + asteroid_parameter_payload.proceduralRocks = 1.5 + asteroid_parameter_payload.brdf = "Lambertian" + asteroid_parameter_payload.reflectanceParameters = [0.5] + asteroid_parameter_payload.meanRadius = 50000 + asteroid_parameter_payload.principalAxisDistortion = [10, 5, 10] + asteroid_parameter_message = messaging.CelestialBodyParametersMsg().write(asteroid_parameter_payload) + module.celestialParametersMessage.subscribeTo(asteroid_parameter_message) + + # Create camera message + camera_payload = messaging.CameraModelMsgPayload() + camera_payload.timeTag = int(0.5*1E9) + camera_payload.cameraId = 1 + camera_payload.parentName = "cielim_sat" + camera_payload.fieldOfView = [20*np.pi/180, 15*np.pi/180] + camera_payload.bodyToCameraMrp = [1/3,-1/3,-1/3] + camera_payload.cameraBodyFramePosition = [1,1,1] + camera_payload.resolution = [4000, 3000] + camera_payload.renderRate = 10000 + camera_payload.focalLength = 0.1 + camera_payload.readNoise = 1 + camera_payload.systemGain = 0.5 + camera_payload.gaussianPointSpreadFunction = 2 + camera_payload.exposureTime = 0.1 + camera_message = messaging.CameraModelMsg().write(camera_payload) + module.cameraModelMessage.subscribeTo(camera_message) + + # Create spacecraft message + spacecraft_payload = messaging.SCStatesMsgPayload() + spacecraft_payload.r_BN_N = [0,0,-3.83E5] + spacecraft_payload.v_BN_N = [4, 5, 6] + spacecraft_payload.sigma_BN = [np.sqrt(2)/2, 0, -np.sqrt(2)/2] + spacecraft_message = messaging.SCStatesMsg().write(spacecraft_payload) + module.spacecraftMessage.subscribeTo(spacecraft_message) + + # Create spice body messages using a similar structure to the factory (for ease of implementation) + class GravBodies: + planetName = "" + isCentralBody = False + planetBodyInMsg = messaging.SpicePlanetStateMsg() + + grav_bodies = [] + bodies_message_list = [] + sun = GravBodies() + sun.planetName = "sun" + sun_data = messaging.SpicePlanetStateMsgPayload() + sun_data.PositionVector = [10E10,0,0] + sun_data.VelocityVector = [4, 5, 6] + sun_data.J20002Pfix = np.eye(3) + sun.planetBodyInMsg = messaging.SpicePlanetStateMsg().write(sun_data) + grav_bodies.append(sun) + bodies_message_list.append(sun_data) + + asteroid_b612 = GravBodies() + asteroid_b612.planetName = "asteroid_b612" + asteroid_b612.isCentralBody = True + asteroid_data = messaging.SpicePlanetStateMsgPayload() + asteroid_data.PositionVector = [0,0,0] + asteroid_data.VelocityVector = [4, 5, 6] + asteroid_data.J20002Pfix = -np.eye(3) + asteroid_b612.planetBodyInMsg = messaging.SpicePlanetStateMsg().write(asteroid_data) + grav_bodies.append(asteroid_b612) + bodies_message_list.append(asteroid_data) + + for gravBody in grav_bodies: + body = cielimInterface.SpiceBody() + body.name = gravBody.planetName + body.isCentralBody = gravBody.isCentralBody + body.spiceStateMessage.subscribeTo(gravBody.planetBodyInMsg) + module.addCelestialBody(body) + + # Run block + unit_test_sim.InitializeSimulation() + unit_test_sim.ConfigureStopTime(int(1e9)) + unit_test_sim.ExecuteSimulation() + module.closeProtobufFile() + + # Read the existing address book. + file_handle = open(module.getSaveFilename(), "rb") + cielim_message = delimited_protobuf.read(file_handle, cielimMessage_pb2.CielimMessage) + + np.testing.assert_equal(cielim_message.currentTime.frameNumber, 0) + np.testing.assert_equal(cielim_message.currentTime.simTimeElapsed, 0) + + np.testing.assert_equal(cielim_message.epoch.year, epoch_payload.year) + np.testing.assert_equal(cielim_message.epoch.month, epoch_payload.month) + np.testing.assert_equal(cielim_message.epoch.day, epoch_payload.day) + np.testing.assert_equal(cielim_message.epoch.hours, epoch_payload.hours) + np.testing.assert_equal(cielim_message.epoch.minutes, epoch_payload.minutes) + np.testing.assert_equal(cielim_message.epoch.seconds, epoch_payload.seconds) + + np.testing.assert_equal(cielim_message.spacecraft.spacecraftName, "cielim_sat") + np.testing.assert_equal(cielim_message.spacecraft.position, spacecraft_payload.r_BN_N) + np.testing.assert_equal(cielim_message.spacecraft.velocity, spacecraft_payload.v_BN_N) + np.testing.assert_equal(cielim_message.spacecraft.attitude, spacecraft_payload.sigma_BN) + + np.testing.assert_equal(cielim_message.camera.cameraId, camera_payload.cameraId) + np.testing.assert_equal(cielim_message.camera.parentName, "cielim_sat") + np.testing.assert_equal(cielim_message.camera.fieldOfView,camera_payload.fieldOfView) + np.testing.assert_equal(cielim_message.camera.resolution, camera_payload.resolution) + np.testing.assert_equal(cielim_message.camera.cameraPositionInBody, camera_payload.cameraBodyFramePosition) + np.testing.assert_equal(cielim_message.camera.bodyFrameToCameraMrp, camera_payload.bodyToCameraMrp) + np.testing.assert_equal(cielim_message.camera.renderRate, camera_payload.renderRate) + np.testing.assert_equal(cielim_message.camera.focalLength, camera_payload.focalLength) + np.testing.assert_equal(cielim_message.camera.exposureTime, camera_payload.exposureTime) + np.testing.assert_equal(cielim_message.camera.pointSpreadFunction, camera_payload.gaussianPointSpreadFunction) + np.testing.assert_equal(cielim_message.camera.systemGain, camera_payload.systemGain) + np.testing.assert_equal(cielim_message.camera.readNoise, camera_payload.readNoise) + + np.testing.assert_equal(cielim_message.camera.renderParameters.cosmicRayStdDeviation, rendering_payload.cosmicRayStdDeviation) + np.testing.assert_equal(cielim_message.camera.renderParameters.enableStrayLight, rendering_payload.enableStrayLight) + np.testing.assert_equal(cielim_message.camera.renderParameters.starField, rendering_payload.starField) + np.testing.assert_equal(cielim_message.camera.renderParameters.rendering, rendering_payload.rendering) + np.testing.assert_equal(cielim_message.camera.renderParameters.enableSmear, rendering_payload.smear) + + i = 0 + for message, body in zip(bodies_message_list, grav_bodies): + name = body.planetName + central = body.isCentralBody + np.testing.assert_equal(cielim_message.celestialBodies[i].bodyName, name) + np.testing.assert_equal(cielim_message.celestialBodies[i].position, message.PositionVector) + np.testing.assert_equal(cielim_message.celestialBodies[i].velocity, message.VelocityVector) + np.testing.assert_equal(cielim_message.celestialBodies[i].attitude, np.array(message.J20002Pfix).flatten()) + np.testing.assert_equal(cielim_message.celestialBodies[i].centralBody, central) + if (name == asteroid_parameter_payload.bodyName): + np.testing.assert_equal(cielim_message.celestialBodies[i].models.shapeModel, asteroid_parameter_payload.shapeModel) + np.testing.assert_equal(cielim_message.celestialBodies[i].models.perlinNoiseStdDeviation, asteroid_parameter_payload.perlinNoise) + np.testing.assert_equal(cielim_message.celestialBodies[i].models.proceduralRocks, asteroid_parameter_payload.proceduralRocks) + np.testing.assert_equal(cielim_message.celestialBodies[i].models.brdfModel, asteroid_parameter_payload.brdf) + np.testing.assert_equal(cielim_message.celestialBodies[i].models.reflectanceParameters, asteroid_parameter_payload.reflectanceParameters) + np.testing.assert_equal(cielim_message.celestialBodies[i].models.meanRadius, asteroid_parameter_payload.meanRadius) + np.testing.assert_equal(cielim_message.celestialBodies[i].models.principalAxisDistortion, asteroid_parameter_payload.principalAxisDistortion) + i += 1 + +if __name__ == "__main__": + test_interface()