Updates

src/tmx/TmxUtils/src/UdpServer.cpp

@@ -198,4 +198,22 @@ namespace tmx::utils {
         return -1;
     }
 
+    std::string UdpServer::stringTimedReceive() {
+        std::vector<char> msg(4000);
+        int num_of_bytes = this->TimedReceive(msg.data(),4000, 5);
+        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();
+
 private:
 	int _socket;
 	int _port;

src/v2i-hub/MUSTSensorDriverPlugin/CMakeLists.txt

@@ -1,4 +1,5 @@
 PROJECT(MUSTSensorDriverPlugin VERSION 7.6.0 LANGUAGES CXX)
+set(CMAKE_CXX_STANDARD 17)
 
 set(TMX_PLUGIN_NAME "Must Sensor Driver Plugin")
 
@@ -11,16 +12,15 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC tmxutils )
 ## Testing ##
 #############
 enable_testing()
-# add_library(${PROJECT_NAME}_lib )
-# target_link_libraries(${PROJECT_NAME}_lib PUBLIC 
-#                                         tmxutils
-
-#                                         )
+add_library(${PROJECT_NAME}_lib src/MUSTSensorDetection.cpp)
+
 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 
+        ${PROJECT_NAME}_lib 
         gtest)

src/v2i-hub/MUSTSensorDriverPlugin/manifest.json

@@ -18,9 +18,24 @@
             "description": "Sending RSU SNMP GET request at every configured interval. Default every 1 second. Unit of measure: second."
         },
         {
-            "key":"RSUConfigurationList",
-            "default":"{ \"RSUS\": [ { \"RSUIp\": \"192.168.XX.XX\", \"SecurityLevel\":\"authPriv\", \"SNMPPort\": \"161\", \"AuthPassPhrase\": \"dummy\", \"User\": \"authOnlyUser\", \"RSUMIBVersion\": \"RSU4.1\" },{ \"RSUIp\": \"192.168.00.XX\", \"SecurityLevel\":\"authPriv\", \"SNMPPort\": \"162\", \"AuthPassPhrase\": \"tester\", \"User\": \"authPrivUser\", \"RSUMIBVersion\": \"RSU4.1\" }] }",
-            "description":"Configurations of the RSUs the V2X hub is connected to."
+            "key":"DetectionReceiverIP",
+            "default":"127.0.0.1",
+            "description":"IP Address V2X-Hub listens for incoming detections"
+        },
+        {
+            "key":"DetectionReceiverPort",
+            "default":"4545",
+            "description":"Port V2X-Hub listens for incoming detections"
+        },
+        {
+            "key":"SensorId",
+            "default":"MUSTSensor1",
+            "description":"Unique Idenifier for Sensor"
+        },
+        {
+            "key":"ProjectionString",
+            "default":"+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs",
+            "description":"Projection string for projecting cartesian detection data into WSG84 coordinates."
         }
     ]
 }

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp

@@ -0,0 +1,59 @@
+#include "MUSTSensorDetection.h"
+
+namespace MUSTSensorDriverPlugin {
+
+
+    MUSTSensorDetection csvToDectection(const std::string &csv ) {
+        MUSTSensorDetection detection;
+        std::vector<std::string> csv_values;
+        std::stringstream ss(csv);
+        while (ss.good()) {
+            std::string substr;
+            std::getline(ss, substr, ',');
+            csv_values.push_back(substr);
+        }
+        if (csv.size() != 9 ){
+            throw std::runtime_error("Failed to parse csv MUST Detection data");
+        }
+        // Read out CSV information
+        detection.cl = fromStringToDetectionClassification(&csv.at(0));
+        detection.position_x = std::stod(&csv.at(1));
+        detection.position_y = std::stod(&csv.at(2));
+        detection.heading = std::stod(&csv.at(3));
+        detection.speed = std::stod(&csv.at(4));
+        detection.size = fromStringToDetectionSize(&csv.at(5));
+        detection.confidence = std::stod(&csv.at(6));
+        detection.trackID = std::stoi(&csv.at(7));
+        detection.timestamp = std::stoll(&csv.at(8));
+        return detection;
+    }
+
+    tmx::messages::simulation::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection) {
+        tmx::messages::simulation::SensorDetectedObject detectedObject;
+        detectedObject.objectId = detection.trackID;
+        detectedObject.position.X = detection.position_x;
+        detectedObject.position.Y = detection.position_y;
+        detectedObject.confidence = detection.confidence;
+        detectedObject.timestamp = detection.timestamp;
+        return detectedObject;
+    }
+    const DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept {
+        try {
+
+            return stringToDetectionClassificationMap.at(str);
+        }
+        catch( const std::out_of_range &e) {
+            return DetectionClassification::NA;
+        }
+    }
+
+    const DetectionSize fromStringToDetectionSize(const std::string &str) noexcept {
+        try {
+
+            return stringToDetectionSizeMap.at(str);
+        }
+        catch( const std::out_of_range &e) {
+            return DetectionSize::NA;
+        }
+    };
+}

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h

@@ -0,0 +1,58 @@
+#pragma once
+#include <string>
+#include <unordered_map>
+#include <stdexcept>      // std::out_of_range
+#include <simulation/SensorDetectedObject.h>
+
+namespace MUSTSensorDriverPlugin {
+
+    enum class DetectionClassification {
+        SEDAN,
+        TRUCK,
+        VAN,
+        NA
+    };
+
+    enum class DetectionSize
+    {
+        SMALL,
+        MEDIUM,
+        LARGE,
+        NA
+    };
+
+    const static std::unordered_map<std::string, DetectionSize> stringToDetectionSizeMap = {
+        { "small", DetectionSize::SMALL},
+        { "medium", DetectionSize::MEDIUM},
+        { "large", DetectionSize::LARGE}
+    };
+
+    const static std::unordered_map<std::string, DetectionClassification> stringToDetectionClassificationMap = {
+        {"sedan", DetectionClassification::SEDAN},
+        {"truck", DetectionClassification::TRUCK},
+        {"van", DetectionClassification::VAN}
+    };
+
+    const DetectionSize fromStringToDetectionSize(const std::string &str) noexcept;
+
+    const DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept;
+
+    struct MUSTSensorDetection {
+        DetectionClassification cl = DetectionClassification::NA;
+        double position_x = 0;
+        double position_y = 0;
+        double heading = 0;
+        double speed = 0;
+        DetectionSize size = DetectionSize::NA;
+        double confidence = 0;
+        unsigned trackID = 0;
+        unsigned long timestamp = 0; 
+
+    };
+
+
+    MUSTSensorDetection csvToDectection(const std::string &csv );
+
+    tmx::messages::simulation::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection);
+
+}

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp

@@ -22,7 +22,7 @@ namespace MUSTSensorDriverPlugin {
 
 	MUSTSensorDriverPlugin::MUSTSensorDriverPlugin(const string &name): PluginClientClockAware(name)
 	{
-
+		mustSensorPacketReceiverThread = std::make_unique<tmx::utils::ThreadTimer>(std::chrono::milliseconds(5));
 		// Subscribe to all messages specified by the filters above.
 		SubscribeToMessages();
 	}
@@ -34,8 +34,37 @@ namespace MUSTSensorDriverPlugin {
 		// Configuration settings are retrieved from the API using the GetConfigValue template class.
 		// This method does NOT execute in the main thread, so variables must be protected
 		// (e.g. using std::atomic, std::mutex, etc.).
+		if (this->IsPluginState(IvpPluginState_registered)) {
+			std::scoped_lock<std::mutex> lock(_configMutex);
+			std::string ip_address;
+			unsigned int port;
+			GetConfigValue<std::string>("DetectionReceiverIP", ip_address);
+			GetConfigValue<uint>("DetectionReceiverPort", port);
+			createUdpServer(ip_address, port);
+			auto message_receiver_tick_id = mustSensorPacketReceiverThread->AddPeriodicTick([this]() {
+            	this->processMUSTSensorDetection();
+       		 	} // end of lambda expression
+        		, std::chrono::milliseconds(5) );
+        	mustSensorPacketReceiverThread->Start();
+		}
+	}
+	void MUSTSensorDriverPlugin::processMUSTSensorDetection(){
+		if (mustSensorPacketReceiver) {
+			MUSTSensorDetection detection = csvToDectection(mustSensorPacketReceiver->stringTimedReceive());
+			tmx::messages::simulation::SensorDetectedObject msg = mustDectionToSensorDetectedObject(detection);
+			PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl;
+       		this->BroadcastMessage<tmx::messages::simulation::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
+		}
 	}
 
+	void MUSTSensorDriverPlugin::createUdpServer(const std::string &address, unsigned int port) {
+		if ( mustSensorPacketReceiver ) {
+			mustSensorPacketReceiver.reset(new UdpServer(address, port));
+		}
+		else {
+			mustSensorPacketReceiver = std::make_unique<UdpServer>(address, port);
+		}
+	}
 
 	void MUSTSensorDriverPlugin::OnConfigChanged(const char *key, const char *value)
 	{

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h

@@ -17,6 +17,12 @@
 
 #include <PluginClientClockAware.h>
 #include <UdpServer.h>
+#include <ThreadWorker.h>
+#include <mutex>
+#include <simulation/SensorDetectedObject.h>
+
+#include "MUSTSensorDetection.h"
+
 
 
 
@@ -35,13 +41,16 @@ namespace MUSTSensorDriverPlugin
             const char* keySensorConnectionStatus = "Sensor Connection Status";
 
             std::unique_ptr<tmx::utils::UdpServer> mustSensorPacketReceiver;
+
+            std::unique_ptr<tmx::utils::ThreadTimer> mustSensorPacketReceiverThread;
             /**
              * @brief Callback triggered on configuration updates
              */
             void UpdateConfigSettings();
             void OnConfigChanged(const char *key, const char *value) override;
-            void initializeMustSensorPacketReceiver();
-
+            void createUdpServer(const std::string &address, unsigned int port);
+
+            void processMUSTSensorDetection();
 
         public:
             /**

src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp

@@ -0,0 +1,22 @@
+#include <gtest/gtest.h>
+#include <MUSTSensorDetection.h>
+
+using namespace MUSTSensorDriverPlugin;
+
+TEST(TestMUSTSensorDetection, fromStringToDetectionSize)
+{
+    ASSERT_EQ(DetectionSize::SMALL, fromStringToDetectionSize("small"));
+    ASSERT_EQ(DetectionSize::MEDIUM, fromStringToDetectionSize("medium"));
+    ASSERT_EQ(DetectionSize::LARGE, fromStringToDetectionSize("large"));
+    ASSERT_EQ(DetectionSize::NA, fromStringToDetectionSize("not_a_size"));
+
+}
+
+TEST(TestMUSTSensorDetection, fromStringToDetectionClassification)
+{
+    ASSERT_EQ(DetectionClassification::SEDAN, fromStringToDetectionClassification("sedan"));
+    ASSERT_EQ(DetectionClassification::VAN, fromStringToDetectionClassification("van"));
+    ASSERT_EQ(DetectionClassification::TRUCK, fromStringToDetectionClassification("truck"));
+    ASSERT_EQ(DetectionClassification::NA, fromStringToDetectionClassification("not_a_classification"));
+
+}