retrieve true COB from interface

Author: tteil

src/simulation/vizard/cielimInterface/cielimInterface.cpp

@@ -288,7 +288,7 @@ void CielimInterface::writeProtobuffer(uint64_t currentSimNanos) {
         /*! - If the camera is requesting periodic images, request them */
         if (this->opNavMode != ClosedLoopMode::OPEN_LOOP &&
             currentSimNanos % this->cameraModelPayload.renderRate == 0 &&
-            this->cameraModelPayload.isOn == 1) {
+            this->cameraModelPayload.renderRate > 0) {
             this->requestImage(currentSimNanos);
 
         }
@@ -319,7 +319,7 @@ void CielimInterface::updateState(uint64_t currentSimNanos) {
  * */
 bool CielimInterface::shouldRequestACameraImage(uint64_t currentSimNanos) const{
     if (currentSimNanos % this->cameraModelPayload.renderRate == 0 &&
-        this->cameraModelPayload.isOn == 1 /*|| this->firstPass < 11*/) {
+        this->cameraModelPayload.renderRate > 0) {
         return true;
     }
     return false;
@@ -341,6 +341,16 @@ void CielimInterface::requestImage(uint64_t currentSimNanos) {
     imagePayload.imageType = 3;
     if (imageData.imageBufferLength > 0) { imagePayload.valid = 1; }
     this->imageOutMessage.write(&imagePayload, this->moduleID, currentSimNanos);
+
+    OpNavCOBMsgPayload centerOfBrightnessPayload = {};
+    centerOfBrightnessPayload.timeTag = currentSimNanos;
+    centerOfBrightnessPayload.cameraID = this->cameraModelPayload.cameraId;
+    if (imageData.centerOfBrightness) {
+        centerOfBrightnessPayload.valid = true;
+        centerOfBrightnessPayload.centerOfBrightness[0] = imageData.centerOfBrightness.value()[0] + 0.5;
+        centerOfBrightnessPayload.centerOfBrightness[1] = imageData.centerOfBrightness.value()[1] + 0.5;
+    }
+    this->centerOfBrightnessOutMessage.write(&centerOfBrightnessPayload, this->moduleID, currentSimNanos);
 }
 
 /*! Get the communication mode

src/simulation/vizard/cielimInterface/cielimInterface.h

@@ -27,6 +27,7 @@
 #include "architecture/msgPayloadDefC/EpochMsgPayload.h"
 #include "architecture/msgPayloadDefC/CelestialBodyParametersMsgPayload.h"
 #include "architecture/msgPayloadDefC/CameraRenderingMsgPayload.h"
+#include "architecture/msgPayloadDefCpp/OpNavCOBMsgPayload.h"
 
 #include "architecture/utilities/rigidBodyKinematics.hpp"
 #include "utilities/vizProtobuffer/cielimMessage.pb.h"
@@ -90,6 +91,7 @@ class CielimInterface : public SysModel {
     ReadFunctor<EpochMsgPayload> epochMessage;    //!< [-] simulation epoch date/time input msg
 
     Message<CameraImageMsgPayload> imageOutMessage;  //!< vector of vizard instrument camera output messages
+    Message<OpNavCOBMsgPayload> centerOfBrightnessOutMessage;  //!< The true image center of brightness output message
 
 private:
     void requestImage(uint64_t currentSimNanos);  //!<   request image and store it in output image message

src/simulation/vizard/cielimInterface/cielimInterface.i

@@ -33,6 +33,7 @@ from Basilisk.architecture.swig_common_model import *
 %include "cielimInterface.h"
 
 %include "architecture/msgPayloadDefCpp/CameraModelMsgPayload.h"
+%include "architecture/msgPayloadDefCpp/OpNavCOBMsgPayload.h"
 %include "architecture/msgPayloadDefC/SCStatesMsgPayload.h"
 %include "architecture/msgPayloadDefC/CameraImageMsgPayload.h"
 %include "architecture/msgPayloadDefC/SpicePlanetStateMsgPayload.h"

src/simulation/vizard/cielimInterface/zmqConnector.cpp

@@ -73,24 +73,25 @@ void ZmqConnector::message_buffer_deallocate(void *data, void *hint)
     free(data);
 }
 
-ImageData ZmqConnector::requestImage(size_t cameraId) {
-    std::string cmdMsg = "REQUEST_IMAGE_";
-    cmdMsg += std::to_string(cameraId);
-    void* img_message = malloc(cmdMsg.length() * sizeof(char));
-    memcpy(img_message, cmdMsg.c_str(), cmdMsg.length());
-
-    auto imageRequestMessage = zmq::message_t(img_message,
-                                              cmdMsg.length(),
-                                              ZmqConnector::message_buffer_deallocate,
-                                              nullptr);
-    this->requesterSocket->send(imageRequestMessage, zmq::send_flags::none);
+ImageData ZmqConnector::requestImage(size_t cameraId, bool shouldReturnImage) {
+    auto cameraIdAsString = std::to_string(cameraId);
+    zmq::message_t msgCameraId(cameraIdAsString);
+    zmq::message_t msgShouldReturnImage(std::to_string(shouldReturnImage).c_str(), sizeof(char));
+    auto res = this->requesterSocket->send(zmq::str_buffer("REQUEST_IMAGE"),
+                                           zmq::send_flags::sndmore);
+    this->requesterSocket->send(msgCameraId, zmq::send_flags::sndmore);
+    this->requesterSocket->send(msgShouldReturnImage, zmq::send_flags::none);
 
     // SAFETY: it's okay to discard these [[nodiscard]] values because
     //   1) the returned optional could only be empty if ZeroMQ fails due to EAGAIN on a non-blocking socket;
     //      but our socket is not non-blocking
     //   2) the returned length in the (present) optional is recoverable from the given message's `.size()` method.
     auto imageLengthMessage = zmq::message_t();
     auto imageMessage = zmq::message_t();
+    auto centerOfBrightnessX = zmq::message_t();
+    auto centerOfBrightnessY = zmq::message_t();
+    auto cobYMsgSize = this->requesterSocket->recv(centerOfBrightnessY, zmq::recv_flags::none);
+    auto cobXMsgSize = this->requesterSocket->recv(centerOfBrightnessX, zmq::recv_flags::none);
     static_cast<void>(this->requesterSocket->recv(imageLengthMessage, zmq::recv_flags::none));
     static_cast<void>(this->requesterSocket->recv(imageMessage, zmq::recv_flags::none));
 
@@ -100,7 +101,17 @@ ImageData ZmqConnector::requestImage(size_t cameraId) {
     void* image = malloc(imageBufferLength*sizeof(char));
     memcpy(image, imagePoint, imageBufferLength*sizeof(char));
 
-    return  ImageData{imageBufferLength, image};
+    auto returnData = ImageData();
+    returnData.imageBuffer = image;
+    returnData.imageBufferLength = imageBufferLength;
+    returnData.centerOfBrightness = std::nullopt;
+
+    if (cobXMsgSize.has_value()) {
+        returnData.centerOfBrightness = Eigen::Vector2d(*centerOfBrightnessX.data<double>(),
+                *centerOfBrightnessY.data<double>());
+    }
+
+    return  returnData;
 }
 
 

src/simulation/vizard/cielimInterface/zmqConnector.h

@@ -22,11 +22,13 @@
 
 #include "cielimMessage.pb.h"
 #include <zmq.hpp>
+#include <Eigen/Core>
 #include <string>
 
 struct ImageData{
     int32_t imageBufferLength;
     void *imageBuffer;
+    std::optional<Eigen::Vector2d> centerOfBrightness;
 };
 
 class ZmqConnector {
@@ -37,7 +39,7 @@ class ZmqConnector {
     void connect();
     [[nodiscard]] bool isConnected() const;
     void send(const cielimMessage::CielimMessage& messagePayload);
-    ImageData requestImage(size_t cameraId);
+    ImageData requestImage(size_t cameraId, bool shoudReturnImage=true);
     void setComPortNumber(std::string &portNumber);
     void ping();