diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp index 800549bc4..95b514d9e 100644 --- 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 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 index da36b7740..313204811 100644 --- 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 index 9a8463dfd..946d67869 100644 --- 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 index d95df460a..a50783d3a 100644 --- 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 ) {