From dd1a41736317749cd8c6227ab8de6fe11f40d689 Mon Sep 17 00:00:00 2001 From: paulbourelly999 <77466294+paulbourelly999@users.noreply.github.com> Date: Thu, 29 Aug 2024 09:18:01 -0400 Subject: [PATCH] Add position and velocity covariance matrix for MUST sensor detection data (#638) # PR Details ## Description This PR adds position and velocity covariance matrix to Sensor Detected Object messages created by the MUST Sensor Plugin based on configured parameters for position variance and velocity variance ## Related Issue [FCP-30 ](https://usdot-carma.atlassian.net/browse/FCP-30) ## Motivation and Context For generation of SDSMs position covariance information is required to enable fusion of sensor information ## How Has This Been Tested? Unit testing and local integration testing ## Types of changes - [x] Defect fix (non-breaking change that fixes an issue) - [ ] New feature (non-breaking change that adds functionality) - [ ] Breaking change (fix or feature that cause existing functionality to change) ## Checklist: - [ ] 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. --- src/v2i-hub/MUSTSensorDriverPlugin/README.md | 7 ++++++ .../MUSTSensorDriverPlugin/manifest.json | 11 +++++++++ .../src/MUSTSensorDetection.cpp | 15 +++++++++++- .../src/MUSTSensorDetection.h | 2 +- .../src/MUSTSensorDriverPlugin.cpp | 4 +++- .../src/MUSTSensorDriverPlugin.h | 4 ++++ .../test/TestMUSTSensorDetection.cpp | 24 +++++++++++++++++-- 7 files changed, 62 insertions(+), 5 deletions(-) diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/README.md b/src/v2i-hub/MUSTSensorDriverPlugin/README.md index fd607eb26..f51522fc3 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/README.md +++ b/src/v2i-hub/MUSTSensorDriverPlugin/README.md @@ -34,6 +34,13 @@ This plugin has several configuration parameters. Below these are listed out as > [!NOTE] > Both **CARMA Streets** and our vehicle automatation **CARMA Platform** rely on the PROJ4 library for projecting data between internal local maps coordinate frames and WSG84. Additional documentation on the projection string can be found in PROJ documentation ()()() +**DetectionPositionVariance**: Variance of the reported positon data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS. + +**DetectionVelocityVariance**: Variance of the reported velocity data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS. + +> [!NOTE] +> Measurement variance can be calcuted from Sensor accuracy data information. For instance if a sensor position accurancy is +/- 0.5 m, assuming a normal distribution and a 95% confidence interval 0.5 m represent 2 standard deviations. Variance is equal the standard deviation squared so a position measurement accuracy of +/- 0.5 m. the variance would be 0.0625 (`(O.5/2)^2`). + After setting these configuration parameters the plugin can simply be enabled. ## Design diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json index dcd15c60f..31348cb46 100755 --- a/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json +++ b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json @@ -37,6 +37,17 @@ "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." + }, + { + "key":"DetectionPositionVariance", + "default":"0.0625", + "description":"Variance of the reported positon data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS." + }, + { + "key":"DetectionVelocityVariance", + "default":"0.0625", + "description":"Variance of the reported velocity data coming from the sensor. Value is used for cooperative perception messages that enable sensor fusion like the SDMS." } + ] } \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp index 0256bb93a..0c36fad1a 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp @@ -29,7 +29,7 @@ namespace MUSTSensorDriverPlugin { return detection; } - tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString) { + tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString, double positionVariance, double velocityVariance) { tmx::messages::SensorDetectedObject detectedObject; detectedObject.set_objectId(detection.trackID); tmx::messages::Position pos(detection.position_x, detection.position_y, 0); @@ -38,8 +38,21 @@ namespace MUSTSensorDriverPlugin { detectedObject.set_timestamp(static_cast(detection.timestamp*1000)); // convert decimal seconds to int milliseconds. detectedObject.set_velocity(headingSpeedToVelocity(detection.heading, detection.speed)); detectedObject.set_type(detectionClassificationToSensorDetectedObjectType(detection.cl)); + std::vector> positionCov(3, std::vector(3,tmx::messages::Covariance(0.0) )); + // Set X and Y position variance in covariance matrix + positionCov[0][0] = tmx::messages::Covariance(positionVariance); + positionCov[1][1] = tmx::messages::Covariance(positionVariance); + detectedObject.set_positionCovariance(positionCov); + + // Set X and Y Velocity variance in covariance matrix + std::vector> velocityCov(3, std::vector(3,tmx::messages::Covariance(0.0) )); + velocityCov[0][0] = tmx::messages::Covariance(velocityVariance); + velocityCov[1][1] = tmx::messages::Covariance(velocityVariance); + detectedObject.set_velocityCovariance(velocityCov); + detectedObject.set_sensorId(std::string(sensorId)); detectedObject.set_projString(std::string(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 index 9140b0252..96de21088 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h @@ -116,7 +116,7 @@ namespace MUSTSensorDriverPlugin { * @param projString std::string describing reference point and WGS84 projection of detection information * @return tmx::messages::SensorDetectedObject */ - tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString); + tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString, double positionVariance, double velocityVariance); /** * @brief Function to convert MUSTSensor provided heading and speed to a velocity vector diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp index 024430b4c..6aa623330 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp @@ -49,6 +49,8 @@ namespace MUSTSensorDriverPlugin { unsigned int port; GetConfigValue("DetectionReceiverIP", ip_address); GetConfigValue("DetectionReceiverPort", port); + GetConfigValue("DetectionPositionVariance", positionVariance); + GetConfigValue("DetectionVelocityVariance", velocityVariance); createUdpServer(ip_address, port); SetStatus(keyMUSTSensorConnectionStatus, "IDLE"); @@ -69,7 +71,7 @@ namespace MUSTSensorDriverPlugin { connected = true; SetStatus(keyMUSTSensorConnectionStatus, "CONNECTED"); } - tmx::messages::SensorDetectedObject msg = mustDetectionToSensorDetectedObject(detection, sensorId, projString); + tmx::messages::SensorDetectedObject msg = mustDetectionToSensorDetectedObject(detection, sensorId, projString,positionVariance, velocityVariance); PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl; this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); } diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h index 3d9e33d8c..af3f2893b 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h @@ -48,6 +48,10 @@ namespace MUSTSensorDriverPlugin std::string projString; + double positionVariance = 0.0; + + double velocityVariance = 0.0; + bool connected = false; // Message receiver thread id diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp index d065d31a3..a72a29609 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp @@ -62,8 +62,8 @@ TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) { detection.timestamp = 1719506355.4; detection.trackID = 324; detection.speed = 5; - - auto sensorDetectedObject = mustDetectionToSensorDetectedObject(detection, "MUSTSensor1", "PROJ String"); + // 0.0625 variance corresponds to 0.25 std. Assuming Normal distribution and a 95% confidence interval corresponds to +/- 0.5m or m/s respectively. + auto sensorDetectedObject = mustDetectionToSensorDetectedObject(detection, "MUSTSensor1", "PROJ String", 0.0625, 0.0625); EXPECT_EQ(detection.trackID, sensorDetectedObject.get_objectId()); EXPECT_DOUBLE_EQ(detection.confidence/100.0, sensorDetectedObject.get_confidence()); @@ -75,6 +75,26 @@ TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) { EXPECT_EQ(1719506355400, sensorDetectedObject.get_timestamp()); EXPECT_EQ("MUSTSensor1", sensorDetectedObject.get_sensorId()); EXPECT_EQ("PROJ String", sensorDetectedObject.get_projString()); + EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_positionCovariance()[0][0].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[0][1].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[0][2].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[1][0].value); + EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_positionCovariance()[1][1].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[1][2].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[2][0].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[2][1].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_positionCovariance()[2][2].value); + + EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_velocityCovariance()[0][0].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[0][1].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[0][2].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[1][0].value); + EXPECT_DOUBLE_EQ( 0.0625, sensorDetectedObject.get_velocityCovariance()[1][1].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[1][2].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[2][0].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[2][1].value); + EXPECT_DOUBLE_EQ( 0.0, sensorDetectedObject.get_velocityCovariance()[2][2].value); + } TEST(TestMUSTSensorDetection, detectionClassificationToSensorDetectedObjectType ) {