Merge pull request #652 from AVSLab/feature/466-running-non-swig-v2

schaubh · web-flow · commit 5d692155a146 · 2024-08-22T07:27:25.000-06:00
Feature/466 running non swig v2
diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst
@@ -133,6 +133,7 @@ Version 2.3.0 (April 5, 2024)
   ``coastOptionBangDuration``. The setter and getter methods for this variable are renamed to reflect this change as
   ``setCoastOptionBangDuration()`` and  ``getCoastOptionBangDuration()``, respectively. See the module documentation
   for the current usage of this parameter and these associated methods.
+- Updated messaging files so that non-swig messages can be subscribed to, and data read out from.  Supports pybind-based messages notably.
 
 
 Version 2.2.1 (Dec. 22, 2023)
diff --git a/src/architecture/messaging/_UnitTest/test_NonSwigMessaging.py b/src/architecture/messaging/_UnitTest/test_NonSwigMessaging.py
@@ -0,0 +1,176 @@
+#
+#  Copyright (c) 2023, 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.
+#
+
+#
+# Purpose:  Test if a C-wrapped input message can be logged with a recorder module and that a C++ module can access its data using non-swig modules
+# Author: Scott Piggott
+# Creation Date:  Aug. 17 2024
+#
+
+
+from Basilisk.architecture import bskLogging
+from Basilisk.architecture import messaging
+from Basilisk.architecture import sim_model
+from Basilisk.moduleTemplates import cModuleTemplate
+from Basilisk.moduleTemplates import cppModuleTemplate
+from Basilisk.utilities import SimulationBaseClass
+from Basilisk.utilities import macros
+from Basilisk.utilities import unitTestSupport as uts
+
+
+def test_RecordingInputMessages():
+    """
+    testing recording a C-wrapped input message with the recorder module
+    """
+
+    bskLogging.setDefaultLogLevel(bskLogging.BSK_WARNING)
+    testFailCount = 0  # zero unit test result counter
+    testMessages = []  # create empty array to store test log messages
+
+    #  Create a sim module as an empty container
+    scSim = SimulationBaseClass.SimBaseClass()
+
+    #  create the simulation process
+    dynProcess = scSim.CreateNewProcess("dynamicsProcess")
+
+    # create the dynamics task and specify the integration update time
+    dynProcess.addTask(scSim.CreateNewTask("dynamicsTask", macros.sec2nano(1.)))
+
+    # create modules
+    mod1 = cModuleTemplate.cModuleTemplate()
+    mod1.ModelTag = "cModule1"
+    scSim.AddModelToTask("dynamicsTask", mod1)
+
+    # Write input data
+    inputData = messaging.CModuleTemplateMsgPayload()
+    inputData.dataVector = [1, 2, 3]
+    inputDataMsg = messaging.CModuleTemplateMsg().write(inputData)
+
+    # Subscribe input message to stand alone message
+    inputAddr = sim_model.getObjectAddress(inputDataMsg)
+    mod1.dataInMsg.subscribeTo(inputAddr)
+
+    # Create recorders tied to IO messages
+    dataInRec = mod1.dataInMsg.recorder()
+    scSim.AddModelToTask("dynamicsTask", dataInRec)
+    dataOutRec = mod1.dataOutMsg.recorder()
+    scSim.AddModelToTask("dynamicsTask", dataOutRec)
+
+    attGuidMsg = messaging.CModuleTemplateMsg_C()
+    attGuidMsgPayload = messaging.CModuleTemplateMsgPayload()
+    attGuidMsg.write(attGuidMsgPayload)
+    messaging.CModuleTemplateMsg_C_addAuthor(mod1.dataOutMsg, attGuidMsg)
+    dataOut2Rec = attGuidMsg.recorder()
+    scSim.AddModelToTask("dynamicsTask", dataOut2Rec)
+
+    # create modules
+    mod1CPP = cppModuleTemplate.CppModuleTemplate()
+    mod1CPP.ModelTag = "cppModule1"
+    scSim.AddModelToTask("dynamicsTask", mod1CPP)
+
+    # Subscribe input message to C-module output message address
+    cOutAddr = sim_model.getObjectAddress(mod1.dataOutMsg)
+    mod1CPP.dataInMsg.subscribeToCAddr(cOutAddr)
+
+    # Create recorders tied to IO messages
+    dataInRecCPP = mod1CPP.dataInMsg.recorder()
+    scSim.AddModelToTask("dynamicsTask", dataInRecCPP)
+    dataOutRecCPP = mod1CPP.dataOutMsg.recorder()
+    scSim.AddModelToTask("dynamicsTask", dataOutRecCPP)
+
+    #  initialize Simulation:
+    scSim.InitializeSimulation()
+
+    #   configure a simulation stop time and execute the simulation run
+    scSim.ConfigureStopTime(macros.sec2nano(1.0))
+    scSim.ExecuteSimulation()
+
+    # reading the module output message show not change the earlier redirection
+    # further, we are testing that the read() command copies the payload from
+    # the stand alone msg to the module output module
+    tempSet = mod1.dataOutMsg.read().dataVector
+    scSim.ConfigureStopTime(macros.sec2nano(2.0))
+    scSim.ExecuteSimulation()
+
+    # print(dataInRec.dataVector)
+    # print(dataOutRec.dataVector)
+    # print(dataOut2Rec.dataVector)
+
+    testFailCount, testMessages = uts.compareArray([inputData.dataVector]*3
+                                                   , dataInRec.dataVector
+                                                   , 0.01
+                                                   , "recorded input message was not correct."
+                                                   , testFailCount
+                                                   , testMessages)
+
+    testFailCount, testMessages = uts.compareArray([[0, 0, 0], [0, 0, 0], [3, 2, 3]]
+                                                   , dataOutRec.dataVector
+                                                   , 0.01
+                                                   , "recorded module output message was not correct."
+                                                   , testFailCount
+                                                   , testMessages)
+
+    testFailCount, testMessages = uts.compareArray([[2, 2, 3], [3, 2, 3], [4, 2, 3]]
+                                                   , dataOut2Rec.dataVector
+                                                   , 0.01
+                                                   , "recorded redirected module output message was not correct."
+                                                   , testFailCount
+                                                   , testMessages)
+
+    testFailCount, testMessages = uts.compareArray([[4., 2., 3.]]
+                                                   , [mod1.dataOutMsg.read().dataVector]
+                                                   , 0.01
+                                                   , "read of module output message was not correct."
+                                                   , testFailCount
+                                                   , testMessages)
+
+    testFailCount, testMessages = uts.compareArray([[4, 2, 3]]
+                                                   , [attGuidMsg.read().dataVector]
+                                                   , 0.01
+                                                   , "read of module redirected output message was not correct."
+                                                   , testFailCount
+                                                   , testMessages)
+    #Check that the input message subscribed with address reflects data right
+    testFailCount, testMessages = uts.compareArray(dataOutRec.dataVector
+                                                   , dataInRecCPP.dataVector
+                                                   , 0.01
+                                                   , "recorded input message from addr was not correct."
+                                                   , testFailCount
+                                                   , testMessages)
+    dataRecExpectation = dataInRecCPP.dataVector
+    dummyVal = 1
+    for i in range(dataRecExpectation.shape[0]):
+        dataRecExpectation[i, 0] += dummyVal
+        dummyVal += 1
+
+    #Check that the output message using input subscribed with address is right
+    testFailCount, testMessages = uts.compareArray(dataRecExpectation
+                                                   , dataOutRecCPP.dataVector
+                                                   , 0.01
+                                                   , "recorded output message from addr was not correct."
+                                                   , testFailCount
+                                                   , testMessages)
+
+    if testFailCount:
+        print(testMessages)
+
+    # each test method requires a single assert method to be called
+    # this check below just makes sure no sub-test failures were found
+    assert testFailCount < 1, testMessages
+
+
+if __name__ == "__main__":
+    test_RecordingInputMessages()
diff --git a/src/architecture/messaging/cMsgCInterface/msg_C.cpp.in b/src/architecture/messaging/cMsgCInterface/msg_C.cpp.in
@@ -114,6 +114,11 @@ void @MSG_AUTOSOURCE_TYPE@_cpp_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, vo
     subscriber->headerPointer->isLinked = 1;    // set output message as linked
 };
 
+//! This method takes an address location as an integer, reinterprets it to be an object, and calls the CPP subscribe method from there
+void @MSG_AUTOSOURCE_TYPE@_addr_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, uint64_t sourceAddr){
+    void *source = reinterpret_cast<void *> (sourceAddr);
+    @MSG_AUTOSOURCE_TYPE@_cpp_subscribe(subscriber, source);
+};
 
 //! Cpp interface to check if subscriber is indeed subscribed to a message (1: subscribed, 0: not subscribed)
 int8_t @MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedTo(@MSG_AUTOSOURCE_TYPE@_C *subscriber, void* source) {
@@ -126,3 +131,11 @@ int8_t @MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedTo(@MSG_AUTOSOURCE_TYPE@_C *subscri
     return (firstCheck && secondCheck);
 
 };
+
+//! Cpp interface to check if subscriber is indeed subscribed to a message-address (1: subscribed, 0: not subscribed)
+int8_t @MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedToAddr(@MSG_AUTOSOURCE_TYPE@_C *subscriber, uint64_t sourceAddr) {
+
+    void *source = reinterpret_cast<void*> (sourceAddr);
+    return (@MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedTo(subscriber, source));
+
+};
diff --git a/src/architecture/messaging/cMsgCInterface/msg_C.h.in b/src/architecture/messaging/cMsgCInterface/msg_C.h.in
@@ -28,8 +28,11 @@ void @MSG_AUTOSOURCE_TYPE@_cpp_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, vo
 
 void @MSG_AUTOSOURCE_TYPE@_C_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, @MSG_AUTOSOURCE_TYPE@_C *source);
 
+void @MSG_AUTOSOURCE_TYPE@_addr_subscribe(@MSG_AUTOSOURCE_TYPE@_C *subscriber, uint64_t sourceAddr);
+
 int8_t @MSG_AUTOSOURCE_TYPE@_C_isSubscribedTo(@MSG_AUTOSOURCE_TYPE@_C *subscriber, @MSG_AUTOSOURCE_TYPE@_C *source);
 int8_t @MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedTo(@MSG_AUTOSOURCE_TYPE@_C *subscriber, void* source);
+int8_t @MSG_AUTOSOURCE_TYPE@_cpp_isSubscribedToAddr(@MSG_AUTOSOURCE_TYPE@_C *subscriber, uint64_t sourceAddr);
 
 void @MSG_AUTOSOURCE_TYPE@_C_addAuthor(@MSG_AUTOSOURCE_TYPE@_C *coowner, @MSG_AUTOSOURCE_TYPE@_C *data);
 
diff --git a/src/architecture/messaging/messaging.h b/src/architecture/messaging/messaging.h
@@ -115,7 +115,21 @@ class ReadFunctor{
         this->initialized = true;           // set input message as linked
         this->headerPointer->isLinked = 1;  // set source output message as linked
     };
-
+    //! Subscribe to the message located at the sourceAddr in memory
+    void subscribeToAddr(uint64_t sourceAddr)
+    {
+        //!Cast a pointer at the sourceAddr and call the regular subscribe
+        Message<messageType> *source =
+            reinterpret_cast<Message<messageType> *> (sourceAddr);
+	    subscribeTo(source);
+    }
+    //! Subscribe to the C message located at the sourceAddr in memory
+    void subscribeToCAddr(uint64_t sourceAddr)
+    {
+        //! Cast a void* pointer and call the regular C-subscribe method
+        void *source = reinterpret_cast<void *> (sourceAddr);
+        subscribeToC(source);
+    }
     //! Subscribe to a C++ message
     void subscribeTo(Message<messageType> *source){
         *this = source->addSubscriber();
@@ -143,6 +157,20 @@ class ReadFunctor{
         return (this->initialized && firstCheck && secondCheck );
 
     };
+    //! Check if self has been subscribed to the message at sourceAddr
+    uint8_t isSubscribedToAddr(uint64_t sourceAddr)
+    {
+        //!Cast a pointer at the sourceAddr and call the regular is-subscribe
+        Message<messageType> *source = reinterpret_cast<Message<messageType> *> (sourceAddr);
+        return(isSubscribedTo(source));
+    }
+    //! Check if self has been subscribed to the message at sourceAddr
+    uint8_t isSubscribedToCAddr(uint64_t sourceAddr)
+    {
+        //!Cast a void* pointer at the sourceAddr and call the regular is-subscribe
+        void *source = reinterpret_cast<void *> (sourceAddr);
+        return(isSubscribedToC(source));
+    }
 
     //! Recorder method description
     Recorder<messageType> recorder(uint64_t timeDiff = 0){return Recorder<messageType>(this, timeDiff);}
diff --git a/src/architecture/messaging/msgAutoSource/cMsgCInterfacePy.i.in b/src/architecture/messaging/msgAutoSource/cMsgCInterfacePy.i.in
@@ -14,6 +14,8 @@ typedef struct {type};
             {type}_C_subscribe(self, source)
         elif type(source) == {type}:
             {type}_cpp_subscribe(self, source)
+        elif type(source) == int: #Note that is assumes it is a uint64_t address in memory
+            {type}_addr_subscribe(self, source)
         else:
             raise Exception('tried to subscribe {type} to another message type')
 
@@ -26,6 +28,8 @@ typedef struct {type};
             return ({type}_C_isSubscribedTo(self, source))
         elif type(source) == {type}:
             return ({type}_cpp_isSubscribedTo(self, source))
+        elif type(source) == int: #Note that this assumes it is a uint64_t address location in memory
+            return ({type}_cpp_isSubscribedToAddr(self, source))
         else:
             return 0
 
diff --git a/src/architecture/utilities/bskLogging.cpp b/src/architecture/utilities/bskLogging.cpp
@@ -104,6 +104,7 @@ void BSKLogger::bskLog(logLevel_t targetLevel, const char* info, ...)
         va_start (args, info);
         vsnprintf(formatMessage, sizeof(formatMessage), info, args);
         printf("%s: %s\n", targetLevelStr, formatMessage);
+	va_end(args);
     }
 }
 

Original file line number	Diff line number	Diff line change
`@@ -104,6 +104,7 @@ void BSKLogger::bskLog(logLevel_t targetLevel, const char* info, ...)`
`104`	`104`	`va_start (args, info);`
`105`	`105`	`vsnprintf(formatMessage, sizeof(formatMessage), info, args);`
`106`	`106`	`printf("%s: %s\n", targetLevelStr, formatMessage);`
	`107`	`+ va_end(args);`
`107`	`108`	`}`
`108`	`109`	`}`
`109`	`110`