Added methods for coverting classification to string and passing

sensorId and projString from configuration
usdot-fhwa-OPS · Jun 20, 2024 · c51f338 · c51f338
1 parent 10e1643
commit c51f338
Show file tree

Hide file tree

Showing 6 changed files with 20 additions and 5 deletions.
diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h
@@ -36,6 +36,7 @@ namespace tmx
             tmx::utils::Point position = tmx::utils::Point();
             tmx::utils::Vector3d velocity = tmx::utils::Vector3d();
             long timestamp = 0;
+
         };
 
     }

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp
@@ -29,14 +29,17 @@ namespace MUSTSensorDriverPlugin {
         return detection;
     }
 
-    tmx::messages::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection) {
+    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, const std::string &sensorId, const std::string &projString) {
         tmx::messages::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;
         detectedObject.velocity = headingSpeedToVelocity(detection.heading, detection.speed);
+        detectedObject.type = detectionClassificationToString(detection.cl);
+        detectedObject.sensorId = sensorId;
+        detectedObject.projString = projString;
         return detectedObject;
     }
     DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept {

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h
@@ -59,7 +59,7 @@ namespace MUSTSensorDriverPlugin {
 
     MUSTSensorDetection csvToDectection(const std::string &csv );
 
-    tmx::messages::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection);
+    tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, const std::string &sensorId, const std::string &projString);
 
     tmx::utils::Vector3d headingSpeedToVelocity(double heading, double speed);
 }
diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp
@@ -36,6 +36,9 @@ namespace MUSTSensorDriverPlugin {
 		// (e.g. using std::atomic, std::mutex, etc.).
 		if (this->IsPluginState(IvpPluginState_registered)) {
 			std::scoped_lock<std::mutex> lock(_configMutex);
+			GetConfigValue<std::string>("ProjectionString", projString);
+			GetConfigValue<std::string>("SensorId", sensorId);
+			// Setup  UDP Server 
 			std::string ip_address;
 			unsigned int port;
 			GetConfigValue<std::string>("DetectionReceiverIP", ip_address);
@@ -51,7 +54,7 @@ namespace MUSTSensorDriverPlugin {
 	void MUSTSensorDriverPlugin::processMUSTSensorDetection(){
 		if (mustSensorPacketReceiver) {
 			MUSTSensorDetection detection = csvToDectection(mustSensorPacketReceiver->stringTimedReceive());
-			tmx::messages::SensorDetectedObject msg = mustDectionToSensorDetectedObject(detection);
+			tmx::messages::SensorDetectedObject msg = mustDetectionToSensorDetectedObject(detection, sensorId, projString);
 			PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl;
        		this->BroadcastMessage<tmx::messages::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
 		}

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h
@@ -43,6 +43,10 @@ namespace MUSTSensorDriverPlugin
             std::unique_ptr<tmx::utils::UdpServer> mustSensorPacketReceiver;
 
             std::unique_ptr<tmx::utils::ThreadTimer> mustSensorPacketReceiverThread;
+
+            std::string sensorId;
+
+            std::string projString;
             /**
              * @brief Callback triggered on configuration updates
              */

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp
@@ -46,7 +46,7 @@ TEST(TestMUSTSensorDetection, csvToDectectionEmptyString ){
     EXPECT_THROW(csvToDectection(valid_csv_data), std::runtime_error);
 }
 
-TEST(TestMUSTSensorDetection, mustDectionToSensorDetectedObject ) {
+TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) {
     using namespace std::chrono;
 
     MUSTSensorDetection detection;
@@ -60,14 +60,18 @@ TEST(TestMUSTSensorDetection, mustDectionToSensorDetectedObject ) {
     detection.trackID = 324;
     detection.speed = 5;
 
-    auto sensorDetectedObject = mustDectionToSensorDetectedObject(detection);
+    auto sensorDetectedObject = mustDetectionToSensorDetectedObject(detection, "MUSTSensor1", "PROJ String");
 
     EXPECT_EQ(detection.trackID, sensorDetectedObject.objectId);
     EXPECT_DOUBLE_EQ(detection.confidence, sensorDetectedObject.confidence);
     EXPECT_DOUBLE_EQ(detection.position_x, sensorDetectedObject.position.X);
     EXPECT_DOUBLE_EQ(detection.position_y, sensorDetectedObject.position.Y);
     EXPECT_NEAR(4.33, sensorDetectedObject.velocity.Y, 0.001);
     EXPECT_NEAR(2.5, sensorDetectedObject.velocity.X, 0.001);
+    EXPECT_STRCASEEQ("SEDAN", sensorDetectedObject.type.c_str());
+    EXPECT_EQ(detection.timestamp, sensorDetectedObject.timestamp);
+    EXPECT_EQ("MUSTSensor1", sensorDetectedObject.sensorId);
+    EXPECT_EQ("PROJ String", sensorDetectedObject.projString);
 }
 
 TEST(TestMUSTSensorDetection, detectionClassificationToString ) {