PR Updates

usdot-fhwa-OPS · Jun 27, 2024 · 022d5f5 · 022d5f5
1 parent 3483b17
commit 022d5f5
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 13 deletions.
diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp
@@ -3,7 +3,7 @@
 namespace MUSTSensorDriverPlugin {
 
 
-    MUSTSensorDetection csvToDectection(const std::string &csv ) {
+    MUSTSensorDetection csvToDetection(const std::string &csv ) {
         MUSTSensorDetection detection;
         std::vector<std::string> csv_values;
         std::stringstream ss(csv);
@@ -13,7 +13,7 @@ namespace MUSTSensorDriverPlugin {
             csv_values.push_back(substr);
         }
         if (csv_values.size() != 9 ){
-            FILE_LOG(tmx::utils::logWARNING) << "CSV data " << csv <<" is size " << csv_values.size() << std::endl;
+            FILE_LOG(tmx::utils::logERROR) << "Data " << csv << " does not match expected csv data format : \'class,x,y,heading,speed,size,confidence,trackId,timestamp\'" << std::endl;
             throw tmx::TmxException("Failed to parse CSV MUST Detection data");
         }
         // Read out CSV information
@@ -78,11 +78,13 @@ namespace MUSTSensorDriverPlugin {
     };
 
     tmx::utils::Vector3d headingSpeedToVelocity(double heading, double speed) {
-        // Convert North East heading to 
+        // Convert North East heading to Angle with 0 at (1, 0) (See README Unit Circle)
         heading = heading - 270;
+        // factor for converting heading from degrees to radians
+        auto headingInRad = M_PI / 180;
         tmx::utils::Vector3d velocity;
-        velocity.X = std::cos(M_PI/180 * heading) * speed;
-        velocity.Y = std::sin(M_PI/180 * heading) * speed;
+        velocity.X = std::cos(headingInRad * heading) * speed;
+        velocity.Y = std::sin(headingInRad * heading) * speed;
         velocity.Z = 0;
         return velocity;
     };

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h
@@ -101,7 +101,7 @@ namespace MUSTSensorDriverPlugin {
      * @return MUSTSensorDetection 
      * @throws tmx::TmxException if string is misformatted.
      */
-    MUSTSensorDetection csvToDectection(const std::string &csv );
+    MUSTSensorDetection csvToDetection(const std::string &csv );
 
     /**
      * @brief Function to convert MUSTSensorDetections to SensorDetectedObject

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp
@@ -68,7 +68,7 @@ namespace MUSTSensorDriverPlugin {
 		if (mustSensorPacketReceiver) {
 			try {
 				PLOG(logDEBUG1) << "Processing MUST Sensor Detection ... " << std::endl;
-				MUSTSensorDetection detection = csvToDectection(mustSensorPacketReceiver->stringTimedReceive());
+				MUSTSensorDetection detection = csvToDetection(mustSensorPacketReceiver->stringTimedReceive());
 				if ( !connected ) {
 					connected = true;
 					SetStatus(keyMUSTSensorConnectionStatus, "CONNECTED");

diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp
@@ -22,9 +22,9 @@ TEST(TestMUSTSensorDetection, fromStringToDetectionClassification)
 
 }
 
-TEST(TestMUSTSensorDetection, csvToDectection ){
+TEST(TestMUSTSensorDetection, csvToDetection ){
     std::string valid_csv_data = "truck,13.3,22.4,30.5,35.8,large,95.1,1,1714374738";
-    auto detection = csvToDectection(valid_csv_data);
+    auto detection = csvToDetection(valid_csv_data);
     EXPECT_EQ(detection.cl, DetectionClassification::TRUCK);
     EXPECT_DOUBLE_EQ(detection.position_x, 13.3);
     EXPECT_DOUBLE_EQ(detection.position_y, 22.4);
@@ -37,13 +37,13 @@ TEST(TestMUSTSensorDetection, csvToDectection ){
 }
 
 TEST(TestMUSTSensorDetection, csvToDectectionInvalidCSV ){
-    std::string valid_csv_data = "truck,13.3,22.4,30.5,35.8,large,95.1,1,1714374738,20";
-    EXPECT_THROW(csvToDectection(valid_csv_data), std::runtime_error);
+    std::string invalid_csv_data = "truck,13.3,22.4,30.5,35.8,large,95.1,1,1714374738,20";
+    EXPECT_THROW(csvToDetection(invalid_csv_data), tmx::TmxException);
 }
 
 TEST(TestMUSTSensorDetection, csvToDectectionEmptyString ){
-    std::string valid_csv_data = "";
-    EXPECT_THROW(csvToDectection(valid_csv_data), std::runtime_error);
+    std::string empty_string = "";
+    EXPECT_THROW(csvToDetection(empty_string), tmx::TmxException);
 }
 
 TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) {