Feature/1096 protobuffer update

.gitignore

@@ -34,8 +34,9 @@ src/cmake-build-debug/*
 supportData/*/__init__.py
 src/utilities/MonteCarlo/__init__.py
 src/utilities/vizProtobuffer/__init__.py
-src/utilities/vizProtobuffer/vizMessage.pb.cc
-src/utilities/vizProtobuffer/vizMessage.pb.h
+src/utilities/vizProtobuffer/*.pb.cc
+src/utilities/vizProtobuffer/*.pb.h
+src/utilities/vizProtobuffer/*.pb2.py
 externalTools/fswAuto/autosetter/sets/*
 externalTools/fswAuto/autowrapper/wraps/*
 **/outputFiles/*

requirements.txt

@@ -1,6 +1,7 @@
 bokeh
 conan>=1.40.1, <2.00.0
 datashader==0.16.3
+delimited-protobuf==1.0.0
 holoviews==1.19.1
 hvplot==0.10.0
 opencv-contrib-python
@@ -9,6 +10,7 @@ pandas
 param==2.1.1
 parse>=1.18.0
 Pillow
+protobuf==3.19.1
 pytest
 pytest-html
 pytest-xdist

src/architecture/msgPayloadDefC/CelestialBodyParametersMsgPayload.h

@@ -28,12 +28,13 @@
 typedef struct {
     char bodyName[MAX_STRING_LENGTH];
     char shapeModel[MAX_STRING_LENGTH];
+    double sigma_BN[3];
     double perlinNoise;
     double proceduralRocks;
     char brdf[MAX_STRING_LENGTH];
     double reflectanceParameters[MAX_PARAMETER_LENGTH];
     double meanRadius;
-    double principalAxisDistortion;
+    double principalAxisDistortion[3];
 }CelestialBodyParametersMsgPayload;
 
 #endif //CELESTIAL_BODY_PARAMETERS

src/architecture/msgPayloadDefCpp/CameraModelMsgPayload.h

@@ -34,10 +34,9 @@ typedef struct {
     double bodyToCameraMrp[3];        //!< [-] MRP defining the orientation of the camera frame relative to the body frame */
     double focalLength;
     int gaussianPointSpreadFunction;  //!< Size of square Gaussian kernel to model point spread function, must be odd
-    double cosmicRayFrequency;  //!< Frequency at which cosmic rays can strike the camera
+    double exposureTime;  //!< [s] Exposure time for each image taken
     double readNoise;  //!< Read noise standard deviation
     double systemGain;  //!< Mapping from current to pixel intensity
-    bool enableStrayLight;  //!< Add basic stray light modelling to images
 }CameraModelMsgPayload;
 
 #endif

src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp

@@ -46,12 +46,15 @@ void FlybyPoint::UpdateState(uint64_t currentSimNanos)
         /*! If this is the first read, seed the algorithm with the solution  */
         auto [r_BN_N, v_BN_N] = this->readRelativeState();
         if (this->firstRead){
+            this->timeOfFirstRead = currentSimNanos*NANO2SEC;
+            this->firstNavPosition = r_BN_N;
+            this->firstNavVelocity = v_BN_N;
             this->computeFlybyParameters(r_BN_N, v_BN_N);
             this->computeRN(r_BN_N, v_BN_N);
             this->firstRead = false;
         }
         /*! Protect against bad new solutions by checking validity */
-        if (this->checkValidity(r_BN_N, v_BN_N)) {
+        else if (this->checkValidity(currentSimNanos, r_BN_N, v_BN_N)) {
             /*! update flyby parameters and guidance frame */
             this->computeFlybyParameters(r_BN_N, v_BN_N);
             this->computeRN(r_BN_N, v_BN_N);
@@ -89,7 +92,7 @@ void FlybyPoint::computeFlybyParameters(Eigen::Vector3d &r_BN_N, Eigen::Vector3d
     this->gamma0 = std::atan(v_BN_N.dot(ur_N) / v_BN_N.dot(ut_N));
 }
 
-bool FlybyPoint::checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const{
+bool FlybyPoint::checkValidity(uint64_t currentSimNanos, Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const{
     bool valid = true;
     Eigen::Vector3d ur_N = r_BN_N.normalized();
     Eigen::Vector3d uv_N = v_BN_N.normalized();
@@ -110,6 +113,11 @@ bool FlybyPoint::checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N)
     if (maxPredictedAcceleration > this->maxAcceleration && this->maxAcceleration > 0) {
         valid = false;
     }
+    double deltaT = currentSimNanos*NANO2SEC - this->timeOfFirstRead;
+    double deltaPositionNorm = (r_BN_N - (this->firstNavPosition + deltaT*this->firstNavVelocity)).norm();
+    if (deltaPositionNorm > this->positionKnowledgeSigma && this->positionKnowledgeSigma > 0) {
+        valid = false;
+    }
 
     return valid;
 }
@@ -232,3 +240,31 @@ double FlybyPoint::getMaximumRateThreshold() const {
 void FlybyPoint::setMaximumRateThreshold(double maxRateThreshold) {
     this->maxRate = maxRateThreshold;
 }
+
+/*! Get the ground based positional knowledge standard deviation
+ @return sigma
+ */
+double FlybyPoint::getPositionKnowledgeSigma() const {
+    return this->positionKnowledgeSigma;
+}
+
+/*! Set the ground based positional knowledge sigma
+ @param sigma
+ */
+void FlybyPoint::setPositionKnowledgeSigma(double positionKnowledgeStd) {
+    this->positionKnowledgeSigma = positionKnowledgeStd;
+}
+
+/*! Get the ground based velocity knowledge standard deviation
+ @return sigma
+ */
+double FlybyPoint::getVelocityKnowledgeSigma() const {
+    return this->velocityKnowledgeSigma;
+}
+
+/*! Set the ground based velocity knowledge sigma
+ @param sigma
+ */
+void FlybyPoint::setVelocityKnowledgeSigma(double velocityKnowledgeStd) {
+    this->velocityKnowledgeSigma = velocityKnowledgeStd;
+}

src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h

@@ -36,7 +36,7 @@ class FlybyPoint: public SysModel {
     void UpdateState(uint64_t CurrentSimNanos) override;
 
     std::tuple<Eigen::Vector3d, Eigen::Vector3d> readRelativeState();
-    bool checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const;
+    bool checkValidity(uint64_t currentSimNanos, Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const;
     void computeFlybyParameters(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N);
     void computeRN(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N);
     std::tuple<Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d> computeGuidanceSolution() const;
@@ -55,13 +55,18 @@ class FlybyPoint: public SysModel {
     void setMaximumAccelerationThreshold(double maxAccelerationThreshold);
     double getMaximumRateThreshold() const;
     void setMaximumRateThreshold(double maxRateThreshold);
+    double getPositionKnowledgeSigma() const;
+    void setPositionKnowledgeSigma(double positionKnowledgeStd);
+    double getVelocityKnowledgeSigma() const;
+    void setVelocityKnowledgeSigma(double velocityKnowledgeStd);
 
-    ReadFunctor<NavTransMsgPayload>  filterInMsg;               //!< input msg relative position w.r.t. asteroid
+    ReadFunctor<NavTransMsgPayload> filterInMsg;               //!< input msg relative position w.r.t. asteroid
     ReadFunctor<EphemerisMsgPayload> asteroidEphemerisInMsg;    //!< input asteroid ephemeris msg
     Message<AttRefMsgPayload> attRefOutMsg;                     //!< Attitude reference output message
 
 private:
     double dt = 0; //!< current time step between last two updates
+    double timeOfFirstRead = 0;  //!< time of first nav solution read
     double timeBetweenFilterData = 0;       //!< time between two subsequent reads of the filter information
     double toleranceForCollinearity = 0;            //!< tolerance for singular conditions when position and velocity are collinear
     int signOfOrbitNormalFrameVector = 1;  //!< Sign of orbit normal vector to complete reference frame
@@ -74,6 +79,10 @@ class FlybyPoint: public SysModel {
     double gamma0 = 0;              //!< flight path angle of the spacecraft at time of read [rad]
     uint64_t lastFilterReadTime = 0;  //!< time of last filter read
     Eigen::Matrix3d R0N;           //!< inertial-to-reference DCM at time of read
+    Eigen::Vector3d firstNavPosition{};           //!< Last position used to create profile
+    Eigen::Vector3d firstNavVelocity{};           //!< Last velocity used to create profile
+    double positionKnowledgeSigma = 0;           //!< Last position used to create profile
+    double velocityKnowledgeSigma = 0;           //!< Last velocity used to create profile
 
 };
 

src/fswAlgorithms/imageProcessing/centerOfBrightness/centerOfBrightness.cpp

@@ -110,8 +110,8 @@ void CenterOfBrightness::UpdateState(uint64_t CurrentSimNanos)
             cobBuffer.valid = true;
             cobBuffer.timeTag = this->sensorTimeTag;
             cobBuffer.cameraID = imageBuffer.cameraID;
-            cobBuffer.centerOfBrightness[0] = cobData.first[0];
-            cobBuffer.centerOfBrightness[1] = cobData.first[1];
+            cobBuffer.centerOfBrightness[0] = cobData.first[0] + 0.5;
+            cobBuffer.centerOfBrightness[1] = cobData.first[1] + 0.5;
             cobBuffer.pixelsFound = static_cast<int32_t> (locations.size());
         }
         cobBuffer.rollingAverageBrightness = averageBrightnessNew;

src/fswAlgorithms/opticalNavigation/cobConverter/cobConverter.cpp

@@ -67,7 +67,7 @@ void CobConverter::Reset(uint64_t CurrentSimNanos)
  */
 void CobConverter::UpdateState(uint64_t CurrentSimNanos)
 {
-    CameraConfigMsgPayload cameraSpecs = this->cameraConfigInMsg();
+    CameraModelMsgPayload cameraSpecs = this->cameraConfigInMsg();
     OpNavCOBMsgPayload cobMsgBuffer = this->opnavCOBInMsg();
     NavAttMsgPayload navAttBuffer = this->navAttInMsg();
     EphemerisMsgPayload ephemBuffer = this->ephemInMsg();
@@ -83,7 +83,7 @@ void CobConverter::UpdateState(uint64_t CurrentSimNanos)
         /*! - Extract rotations from relevant messages */
         double CB[3][3];
         double BN[3][3];
-        MRP2C(cameraSpecs.sigma_CB, CB);
+        MRP2C(cameraSpecs.bodyToCameraMrp, CB);
         Eigen::Matrix3d dcm_CB = c2DArray2EigenMatrix3d(CB);
         MRP2C(navAttBuffer.sigma_BN, BN);
         Eigen::Matrix3d dcm_BN = c2DArray2EigenMatrix3d(BN);
@@ -92,7 +92,7 @@ void CobConverter::UpdateState(uint64_t CurrentSimNanos)
 
         /*! - camera parameters */
         double alpha = 0;
-        double fieldOfView = cameraSpecs.fieldOfView;
+        double fieldOfView = cameraSpecs.fieldOfView[0];
         double resolutionX = cameraSpecs.resolution[0];
         double resolutionY = cameraSpecs.resolution[1];
         double pX = 2.*tan(fieldOfView/2.0);
@@ -234,7 +234,7 @@ void CobConverter::UpdateState(uint64_t CurrentSimNanos)
         comMsgBuffer.objectPixelRadius = int(Rc);
         comMsgBuffer.phaseAngle = alphaPA;
         comMsgBuffer.sunDirection = phi;
-        comMsgBuffer.cameraID = cameraSpecs.cameraID;
+        comMsgBuffer.cameraID = cameraSpecs.cameraId;
         comMsgBuffer.timeTag = cobMsgBuffer.timeTag;
         comMsgBuffer.valid = validCOM;
     }

src/fswAlgorithms/opticalNavigation/cobConverter/cobConverter.h

@@ -25,7 +25,7 @@
 #include "architecture/utilities/avsEigenSupport.h"
 #include "architecture/messaging/messaging.h"
 
-#include "architecture/msgPayloadDefC/CameraConfigMsgPayload.h"
+#include "architecture/msgPayloadDefCpp/CameraModelMsgPayload.h"
 #include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
 #include "architecture/msgPayloadDefC/EphemerisMsgPayload.h"
 #include "architecture/msgPayloadDefCpp/OpNavCOBMsgPayload.h"
@@ -38,6 +38,7 @@
 #include "architecture/utilities/linearAlgebra.h"
 #include "architecture/utilities/avsEigenMRP.h"
 #include "architecture/utilities/bskLogging.h"
+#include "architecture/utilities/macroDefinitions.h"
 
 enum class PhaseAngleCorrectionMethod {NoCorrection, Lambertian, Binary};
 
@@ -78,7 +79,7 @@ class CobConverter: public SysModel {
     Message<OpNavCOMMsgPayload> opnavCOMOutMsg;
     ReadFunctor<OpNavCOBMsgPayload> opnavCOBInMsg;
     ReadFunctor<FilterMsgPayload> opnavFilterInMsg;
-    ReadFunctor<CameraConfigMsgPayload> cameraConfigInMsg;
+    ReadFunctor<CameraModelMsgPayload> cameraConfigInMsg;
     ReadFunctor<NavAttMsgPayload> navAttInMsg;
     ReadFunctor<EphemerisMsgPayload> ephemInMsg;
 

src/fswAlgorithms/opticalNavigation/cobConverter/cobConverter.i

@@ -33,12 +33,9 @@ from Basilisk.architecture.swig_common_model import *
 
 %include "cobConverter.h"
 
-%include "architecture/msgPayloadDefC/CameraConfigMsgPayload.h"
-struct CameraConfigMsg_C;
+%include "architecture/msgPayloadDefCpp/CameraModelMsgPayload.h"
 %include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
-struct NavAttMsg_C;
 %include "architecture/msgPayloadDefC/EphemerisMsgPayload.h"
-struct EphemerisMsg_C;
 
 %include "architecture/msgPayloadDefCpp/OpNavUnitVecMsgPayload.h"
 %include "architecture/msgPayloadDefCpp/OpNavCOBMsgPayload.h"

src/simulation/sensors/camera/_UnitTest/test_camera.py

@@ -167,10 +167,9 @@ def cameraTest(show_plots, image, gauss, darkCurrent, saltPepper, cosmic, blurSi
     module.setImageCadence(1)
     module.setFocalLength(2.1)
     module.setGaussianPointSpreadFunction(3)
-    module.setCosmicRayFrequency(2.2)
     module.setReadNoise(2.3)
     module.setSystemGain(3.3)
-    module.setEnableStrayLight(True)
+    module.setExposureTime(1.1)
 
     # Noise parameters
     module.gaussian = gauss
@@ -228,14 +227,12 @@ def cameraTest(show_plots, image, gauss, darkCurrent, saltPepper, cosmic, blurSi
                             "Test failed camera focal length")
     np.testing.assert_equal(dataLogCameraModel.gaussianPointSpreadFunction, module.getGaussianPointSpreadFunction(),
                             "Test failed size of square Gaussian kernel to model point spread function")
-    np.testing.assert_equal(dataLogCameraModel.cosmicRayFrequency, module.getCosmicRayFrequency(),
-                            "Test failed frequency at which cosmic rays can strike the camera")
     np.testing.assert_equal(dataLogCameraModel.readNoise, module.getReadNoise(),
                             "Test failed read noise standard deviation")
     np.testing.assert_equal(dataLogCameraModel.systemGain, module.getSystemGain(),
                             "System failed mapping from current to pixel intensity")
-    np.testing.assert_equal(dataLogCameraModel.enableStrayLight, module.getEnableStrayLight(),
-                            "Test failed add basic stray light modelling to images")
+    np.testing.assert_equal(dataLogCameraModel.exposureTime, module.getExposureTime(),
+                            "System failed exposure time")
 
     #  Error check for corruption
     err = np.linalg.norm(np.linalg.norm(input_image, axis=2) - np.linalg.norm(output_image, axis=2)) / np.linalg.norm(

src/simulation/sensors/camera/camera.cpp

@@ -346,10 +346,9 @@ void Camera::UpdateState(uint64_t currentSimNanos)
     eigenVector3d2CArray(this->bodyToCameraMrp, cameraModelMsg.bodyToCameraMrp);
     cameraModelMsg.focalLength = this->focalLength;
     cameraModelMsg.gaussianPointSpreadFunction = this->gaussianPointSpreadFunction;
-    cameraModelMsg.cosmicRayFrequency = this->cosmicRayFrequency;
     cameraModelMsg.readNoise = this->readNoise;
     cameraModelMsg.systemGain = this->systemGain;
-    cameraModelMsg.enableStrayLight = this->enableStrayLight;
+    cameraModelMsg.exposureTime = this->exposureTime;
 
     /*! - Update the camera config data no matter if an image is present*/
     this->cameraConfigOutMsg.write(&cameraMsg, this->moduleID, currentSimNanos);
@@ -489,7 +488,7 @@ Eigen::Vector2i Camera::getResolution() const {
     @return void
     */
 void Camera::setImageCadence(const uint64_t& cameraImageCadence) {
-    this->renderRate = cameraImageCadence;
+    this->imageCadence = cameraImageCadence;
 }
 
 /*! Get the frame time interval at which to capture images in units of nanosecond
@@ -577,23 +576,6 @@ int Camera::getGaussianPointSpreadFunction() const {
     return this->gaussianPointSpreadFunction;
 }
 
-/*! Set the frequency at which cosmic rays can strike the camera
-    @param cameraCosmicRayFrequency double
-    @return void
-    */
-
-void Camera::setCosmicRayFrequency(const double cameraCosmicRayFrequency) {
-    this->cosmicRayFrequency = cameraCosmicRayFrequency;
-}
-
-/*! Get the frequency at which cosmic rays can strike the camera
-    @return double cosmicRayFrequency
-    */
-
-double Camera::getCosmicRayFrequency() const{
-    return this->cosmicRayFrequency;
-}
-
 /*! Set the read noise standard deviation
     @param cameraReadNoise double
     @return void
@@ -628,19 +610,19 @@ double Camera::getSystemGain() const {
     return this->systemGain;
 }
 
-/*! Set the mapping from current to pixel intensity
-    @param cameraEnableStrayLight bool
+/*! Set the camera epxosure time in seconds
+    @param openExposureTime double
     @return void
     */
 
-void Camera::setEnableStrayLight(const bool cameraEnableStrayLight) {
-    this->enableStrayLight = cameraEnableStrayLight;
+void Camera::setExposureTime(const double openExposureTime) {
+    this->exposureTime = openExposureTime;
 }
 
-/*! Set the mapping from current to pixel intensity
-    @return bool enableStrayLight
+/*! Get the mapping from current to pixel intensity
+    @return double exposureTime
     */
 
-bool Camera::getEnableStrayLight() const {
-    return this->enableStrayLight;
+double Camera::getExposureTime() const {
+    return this->exposureTime;
 }

src/simulation/sensors/camera/camera.h

@@ -76,14 +76,13 @@ class Camera: public SysModel {
     double getFocalLength() const;
     void setGaussianPointSpreadFunction(int cameraGaussianPointSpreadFunction);
     int getGaussianPointSpreadFunction() const;
-    void setCosmicRayFrequency(double cameraCosmicRayFrequency);
-    double getCosmicRayFrequency() const;
     void setReadNoise(double cameraReadNoise);
     double getReadNoise() const;
     void setSystemGain(double cameraGain);
     double getSystemGain() const;
-    void setEnableStrayLight(bool cameraEnablesStrayLight);
-    bool getEnableStrayLight() const;
+    void setExposureTime(double exposureTime);
+    double getExposureTime() const;
+
 
 private:
     std::string parentSpacecraftName{};  //!< [-] Name of the parent body to which the camera should be attached
@@ -96,10 +95,9 @@ class Camera: public SysModel {
     Eigen::Vector3d bodyToCameraMrp{};        //!< [-] MRP defining the orientation of the camera frame relative to the body frame
     double focalLength{};   //!< Camera focal length
     int gaussianPointSpreadFunction{};  //!< Size of square Gaussian kernel to model point spread function, must be odd
-    double cosmicRayFrequency{};  //!< Frequency at which cosmic rays can strike the camera
     double readNoise{};  //!< Read noise standard deviation
     double systemGain{};  //!< Mapping from current to pixel intensity
-    bool enableStrayLight{};  //!< Add basic stray light modelling to images
+    double exposureTime{1};  //!< Mapping from current to pixel intensity
 
 public:
     std::string filename{};                //!< Filename for module to read an image directly

src/simulation/vizard/cielimInterface/Custom.cmake

@@ -1,3 +1,3 @@
 find_package(cppzmq CONFIG REQUIRED)
 target_link_libraries(${TARGET_NAME} PRIVATE cppzmq::cppzmq)
-target_link_libraries(${TARGET_NAME} PRIVATE vizMessage)
+target_link_libraries(${TARGET_NAME} PRIVATE cielimMessage)