MUST Sensor Driver (#618)

<!-- Thanks for the contribution, this is awesome. -->

# PR Details
## Description
### MUST Sensor Driver Plugin
Adding MUST Sensor Driver to V2X-Hub. Documentation on MUST Sensor
Driver will be included in README. This new plugin is responsible for
consuming UDP CSV (Comma Separated Values) string packets that represent
detections from the MUST Sensor. These will be converted into
SensorDetectedObject which can be forwarded to the **CARMA Streets**
**Sensor Data Sharing Service** to create SDSMs (Sensor Data Sharing
Messages).
### Sensor Detected Object
This PR includes updates to the **SensorDetectedObject** TMX message
including removing references to simulation since this message will no
longer be exclusively used in simulation. This PR also includes adding
fields and JSON serialization methods to the **SensorDetectedObject**.
This was previously unnecessary since incoming messages from CDASim (our
simulation environment) were already JSON strings and were just passed
via payload.

### UDP Server
This PR also includes updates to the UDPServer class in TMX Utils. It
provides a method to directly consume string payloads from the
UDPServer. Many users of the UDP Server have to write this logic when
they use the UDP Server. Adding it to the class is in an effort to
reduce duplication of this logic across V2X-Hub.

### Future Improvements
This PR further highlights the needs to address/fix
#561 which currently
prevents from correct JSON serialization and deserialization of TMX
messages and prevents defining messages with nested objects.
<!--- Describe your changes in detail -->

## Related Issue
[FCP-3](https://usdot-carma.atlassian.net/browse/FCP-3)
<!--- This project only accepts pull requests related to open issues -->
<!--- If suggesting a new feature or change, please discuss it in an
issue first -->
<!--- If fixing a bug, there should be an issue describing it with steps
to reproduce -->
<!--- Please link to the issue here: -->

## Motivation and Context
Freight Cooperative Perception
<!--- Why is this change required? What problem does it solve? -->

## How Has This Been Tested?
Unit testing and script based integration testing. For instructions for
script based integration testing refer to **Functionality Testing**
section of the README document.
<!--- Please describe in detail how you tested your changes. -->
<!--- Include details of your testing environment, and the tests you ran
to -->
<!--- see how your change affects other areas of the code, etc. -->

## Types of changes

<!--- What types of changes does your code introduce? Put an `x` in all
the boxes that apply: -->

- [ ] Defect fix (non-breaking change that fixes an issue)
- [x] New feature (non-breaking change that adds functionality)
- [ ] Breaking change (fix or feature that cause existing functionality
to change)

## Checklist:

<!--- Go over all the following points, and put an `x` in all the boxes
that apply. -->
<!--- If you're unsure about any of these, don't hesitate to ask. We're
here to help! -->

- [x] I have added any new packages to the sonar-scanner.properties file
- [x] My change requires a change to the documentation.
- [x] I have updated the documentation accordingly.
- [x] I have read the **CONTRIBUTING** document.
[V2XHUB Contributing
Guide](https://github.com/usdot-fhwa-OPS/V2X-Hub/blob/develop/Contributing.md)
- [x] I have added tests to cover my changes.
- [x] All new and existing tests passed.

.sonarqube/sonar-scanner.properties

@@ -57,7 +57,8 @@ sonar.modules=  PedestrianPlugin, \
   ERVCloudForwardingPlugin, \
   CDASimAdapter, \
   RSUHealthMonitorPlugin, \
-  TelematicBridgePlugin
+  TelematicBridgePlugin, \
+  MUSTSensorDriverPlugin
 
 
 
@@ -86,6 +87,7 @@ ERVCloudForwardingPlugin.sonar.projectBaseDir         =src/v2i-hub/ERVCloudForwa
 CDASimAdapter.sonar.projectBaseDir         =src/v2i-hub/CDASimAdapter
 RSUHealthMonitorPlugin.sonar.projectBaseDir     =src/v2i-hub/RSUHealthMonitorPlugin
 TelematicBridgePlugin.sonar.projectBaseDir     =src/v2i-hub/TelematicBridgePlugin
+MUSTSensorDriverPlugin.sonar.projectBaseDir     =src/v2i-hub/MUSTSensorDriverPlugin
 
 
 
@@ -125,6 +127,7 @@ RSUHealthMonitorPlugin.sonar.sources    =src
 RSUHealthMonitorPlugin.sonar.exclusions =test/**
 TelematicBridgePlugin.sonar.sources    =src
 TelematicBridgePlugin.sonar.exclusions =test/**
+MUSTSensorDriverPlugin.sonar.sources  =src
 
 # Tests
 # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated.
@@ -154,3 +157,4 @@ ERVCloudForwardingPlugin.sonar.tests=test
 CDASimAdapter.sonar.tests=test
 RSUHealthMonitorPlugin.sonar.tests=test
 TelematicBridgePlugin.sonar.tests=test
+MUSTSensorDriverPlugin.sonar.tests=test

src/tmx/Messages/include/SensorDetectedObject.h

@@ -0,0 +1,54 @@
+#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_
+#define INCLUDE_SIMULATED_SensorDetectedObject_H_
+
+#include <tmx/messages/message.hpp>
+#include <MessageTypes.h>
+#include <Vector3d.h>
+#include <Point.h>
+
+namespace tmx
+{
+    namespace messages
+    {
+
+        /**
+         * This SensorDetectedObject is used to communicate the sensor detected object information with various applications.
+         * This message is the generic representation of a sensor detection.
+         */
+        class SensorDetectedObject : public tmx::message
+        {
+        public:
+            SensorDetectedObject(){};
+            SensorDetectedObject(const tmx::message_container_type &contents) : tmx::message(contents) {};
+            ~SensorDetectedObject(){};
+            // Message type for routing this message through TMX core
+            static constexpr const char *MessageType = MSGTYPE_APPLICATION_STRING;
+
+            // // Message sub type for routing this message through TMX core
+            static constexpr const char *MessageSubType = MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING;
+
+            // TODO: Convert this member variable to std::attributes and handle nested object and arrays. (see [CloudHeartbeatMessage.h](./CloudHearbeatMessage.h) array_attribute )
+
+            // Classification of detected object
+            std::string type = "";
+            // Confidence of type classification
+            double confidence = 0.0;
+            // Unique indentifier of sensor reporting detection
+            std::string sensorId = "";
+            // String describing projection used to convert cartesian data to WGS84 data 
+            std::string projString = "";
+            // Unique identifier of detected object
+            int objectId = 0;
+            // Cartesian positiion of object. Assumed to be ENU coordinate frame.
+            tmx::utils::Point position = tmx::utils::Point();
+            // Cartesian velocity vector of object. Assumed to be ENU coordinate frame.
+            tmx::utils::Vector3d velocity = tmx::utils::Vector3d();
+            // Epoch time in milliseconds
+            long timestamp = 0;
+
+        };
+
+    }
+
+}; // namespace tmx
+#endif

src/tmx/TmxUtils/src/UdpServer.cpp

@@ -198,4 +198,22 @@ namespace tmx::utils {
         return -1;
     }
 
+    std::string UdpServer::stringTimedReceive(int maxWait_ms) {
+        std::vector<char> msg(4000);
+        int num_of_bytes = this->TimedReceive(msg.data(),4000, maxWait_ms);
+        if (num_of_bytes > 0 ) {
+            msg.resize(num_of_bytes);
+            std::string ret(msg.data());
+            FILE_LOG(logDEBUG) << "UDP Server message received : " << ret << " of size " << num_of_bytes << std::endl;
+            return ret;
+        }
+        else if ( num_of_bytes == 0 ) {
+            throw UdpServerRuntimeError("Received empty message!");
+        }
+        else {
+            throw UdpServerRuntimeError("Listen timed out after 5 ms!");
+        }
+        return "";
+    }
+
 } // namespace tmx::utils

src/tmx/TmxUtils/src/UdpServer.h

@@ -13,6 +13,7 @@
 #include <netdb.h>
 #include <stdexcept>
 #include <tmx/TmxException.hpp>
+#include <PluginLog.h>
 
 namespace tmx {
 namespace utils {
@@ -36,6 +37,8 @@ class UdpServer
 	virtual int Receive(char *msg, size_t maxSize);
 	virtual int TimedReceive(char *msg, size_t maxSize, int maxWait_ms);
 
+	virtual std::string stringTimedReceive(int maxWait_ms=5);
+
 private:
 	int _socket;
 	int _port;

src/tmx/TmxUtils/src/Vector3d.h

@@ -0,0 +1,19 @@
+#pragma once
+
+namespace tmx::utils {
+
+
+    /// Three dimensional Vector
+    using Vector3d = struct Vector3d
+    {
+        Vector3d() : X(0), Y(0), Z(0) {}
+
+        Vector3d(double x, double y, double z = 0.0):
+            X(x), Y(y), Z(z) { }
+
+        double X;
+        double Y;
+        double Z;
+    };
+
+} // namespace tmx::utils

src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp

@@ -25,7 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) :
 	AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage);
 	AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage);
 	AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage);
-	AddMessageFilter < simulation::SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage );
+	AddMessageFilter < SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage );
 
 	SubscribeToMessages();
 }
@@ -704,7 +704,7 @@ void CARMAStreetsPlugin::SubscribeSDSMKafkaTopic(){
 
 }
 
-void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg)
+void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(SensorDetectedObject &msg, routeable_message &routeableMsg)
 {
 	// TODO: This is a temporary fix for tmx message container property tree
 	// serializing all attributes as strings. This issue needs to be fixed but

src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h

@@ -20,7 +20,7 @@
 #include <kafka/kafka_client.h>
 #include <kafka/kafka_consumer_worker.h>
 #include "JsonToJ2735SSMConverter.h"
-#include <simulation/SensorDetectedObject.h>
+#include <SensorDetectedObject.h>
 #include "JsonToJ3224SDSMConverter.h"
 #include "J3224ToSDSMJsonConverter.h"
 #include "PluginClientClockAware.h"
@@ -56,7 +56,7 @@ class CARMAStreetsPlugin: public PluginClientClockAware {
 	 * @param msg Detected object received from TMX bus.
 	 * @param routeableMsg routeable_message for detected object.
 	 */
-	void HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg,  routeable_message &routeableMsg);
+	void HandleSimulatedSensorDetectedMessage(SensorDetectedObject &msg,  routeable_message &routeableMsg);
 	/**
 	 * @brief Overide PluginClientClockAware HandleTimeSyncMessage to producer TimeSyncMessage to kafka for CARMA Streets Time Synchronization.
 	 * @param msg TimeSyncMessage received by plugin when in simulation mode. Message provides current simulation time to all processes.

src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp

@@ -87,9 +87,9 @@ namespace CDASimAdapter{
 
     }
 
-    void CDASimAdapter::forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg) {
+    void CDASimAdapter::forward_simulated_detected_message(tmx::messages::SensorDetectedObject &msg) {
         PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl;
-        this->BroadcastMessage<tmx::messages::simulation::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
+        this->BroadcastMessage<tmx::messages::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
     }
 
     bool CDASimAdapter::connect() {

src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp

@@ -132,7 +132,7 @@ namespace CDASimAdapter{
         tmx::messages::TimeSyncMessage msg;
         msg.clear();
         if (time_sync_listener) {
-            std::string str_msg = consume_server_message(time_sync_listener);
+            std::string str_msg = time_sync_listener->stringTimedReceive();
             msg.set_contents( str_msg );
         }
         else {
@@ -142,13 +142,13 @@ namespace CDASimAdapter{
 
     }
 
-    tmx::messages::simulation::SensorDetectedObject CDASimConnection::consume_sensor_detected_object_message() const
+    tmx::messages::SensorDetectedObject CDASimConnection::consume_sensor_detected_object_message() const
     {
-        tmx::messages::simulation::SensorDetectedObject externalObj;
+        tmx::messages::SensorDetectedObject externalObj;
         externalObj.clear();
         if(sensor_detected_object_listener)
         {
-            std::string str_msg = consume_server_message(sensor_detected_object_listener);
+            std::string str_msg = sensor_detected_object_listener->stringTimedReceive();
             externalObj.set_contents(str_msg);
         }
         else
@@ -180,23 +180,6 @@ namespace CDASimAdapter{
         return "";
     }
 
-    std::string CDASimConnection::consume_server_message( const std::shared_ptr<UdpServer> _server) const {
-        std::vector<char> msg(4000);
-        int num_of_bytes = _server->TimedReceive(msg.data(),4000, 5);
-        if (num_of_bytes > 0 ) {
-            msg.resize(num_of_bytes);
-            std::string ret(msg.data());
-            PLOG(logDEBUG) << "UDP Server message received : " << ret << " of size " << num_of_bytes << std::endl;
-            return ret;
-        }
-        else if ( num_of_bytes == 0 ) {
-            throw UdpServerRuntimeError("Received empty message!");
-        }
-        else {
-            throw UdpServerRuntimeError("Listen timed out after 5 ms!");
-        }
-        return "";
-    }
 
     std::string CDASimConnection::consume_v2x_message_from_simulation() const {
         if ( carma_simulation_listener) {

src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp

@@ -93,7 +93,7 @@ namespace CDASimAdapter
          * @brief Forward simulated sensor detected object message to TMX message bus for other V2X-Hub Plugin 
          * @param msg simulation::SensorDetectedObject.
          */
-        void forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg);
+        void forward_simulated_detected_message(tmx::messages::SensorDetectedObject &msg);
         /**
          * @brief Method to start thread timer for regular interval actions lauched on seperate thread.
          */

src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp

@@ -4,7 +4,7 @@
 #include <tmx/tmx.h>
 #include <Point.h>
 #include <TimeSyncMessage.h>
-#include <simulation/SensorDetectedObject.h>
+#include <SensorDetectedObject.h>
 #include <jsoncpp/json/json.h>
 #include <PluginLog.h>
 #include <gtest/gtest.h>
@@ -64,12 +64,6 @@ namespace CDASimAdapter {
              * @param _client UDP client to forward message with.
              */
             void forward_message(const std::string &v2x_message, const std::shared_ptr<tmx::utils::UdpClient> _client ) const ;
-            /**
-             * @brief Method to consume incoming std::string message from UDP Server.
-             * @param _server UDP Server to consume string message from.
-             * @return string of message.
-             */
-            std::string consume_server_message( const std::shared_ptr<tmx::utils::UdpServer> _server ) const;
 
             /**
              * @brief Method to consume incoming std::string message in hex format from UDP Server.
@@ -88,7 +82,7 @@ namespace CDASimAdapter {
              * To populate the simulation external object, this JSON string has to follow this specification: https://usdot-carma.atlassian.net/wiki/spaces/CRMSIM/pages/2563899417/Detected+Objects+Specification#CARMA-Street-and-V2xHub
              * @return simulation::SensorDetectedObject.
              */
-            tmx::messages::simulation::SensorDetectedObject consume_sensor_detected_object_message() const;
+            tmx::messages::SensorDetectedObject consume_sensor_detected_object_message() const;
             /**
              * @brief Perform handshake with CARMA-Simulation. Will return true on successful handshakes and false if 
              * unsuccessful. As part of the handshake should set simulation_v2x_port for forwarding v2x messages to simulation,

src/v2i-hub/CDASimAdapter/test/TestCDASimConnection.cpp

@@ -59,25 +59,6 @@ namespace CDASimAdapter {
         connection->forward_message(test_message, client);
     } 
 
-    TEST_F( TestCDASimConnection, consume_msg){
-
-        std::shared_ptr<MockUpdServer> server = std::make_shared<MockUpdServer>();
-        char *msg_data  = new char();
-        char test_string[] = "Test Message";
-        EXPECT_CALL( *server, TimedReceive(_, _, _) ).Times(2).
-            WillOnce(testing::DoAll(Return(-1))).
-            WillRepeatedly( testing::DoAll( SetArrayArgument<0>(test_string, test_string + strlen(test_string) + 1),Return(10)));
-        ASSERT_THROW(connection->consume_server_message(server), UdpServerRuntimeError);
-
-        std::string msg = connection->consume_server_message(server);
-
-        std::string compare_str;
-        compare_str = test_string;
-        ASSERT_EQ(compare_str.compare( msg ) , 0);
-        delete msg_data;
-
-    }
-
     TEST_F( TestCDASimConnection, setup_upd_connection) {
         ASSERT_TRUE(connection->setup_udp_connection("127.0.0.1", "127.0.0.1", 4567, 4568, 4569, 4570));
     }

src/v2i-hub/MUSTSensorDriverPlugin/CMakeLists.txt

@@ -0,0 +1,27 @@
+PROJECT(MUSTSensorDriverPlugin VERSION 7.6.0 LANGUAGES CXX)
+set(CMAKE_CXX_STANDARD 17)
+
+set(TMX_PLUGIN_NAME "Must Sensor Driver Plugin")
+
+
+BuildTmxPlugin()
+
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC tmxutils )
+
+#############
+## Testing ##
+#############
+enable_testing()
+add_library(${PROJECT_NAME}_lib src/MUSTSensorDetection.cpp)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME}_lib PUBLIC tmxutils )
+
+set(BINARY ${PROJECT_NAME}_test)
+file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp)
+set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
+add_executable(${BINARY} ${TEST_SOURCES})
+add_test(NAME ${BINARY} COMMAND ${BINARY})
+TARGET_INCLUDE_DIRECTORIES(${BINARY} PUBLIC /usr/local/lib src/) 
+
+target_link_libraries(${BINARY} PUBLIC 
+        ${PROJECT_NAME}_lib 
+        gtest)