init

usdot-fhwa-OPS · Aug 6, 2024 · 78f4bb6 · 78f4bb6
1 parent 4847aa7
commit 78f4bb6
Show file tree

Hide file tree

Showing 6 changed files with 218 additions and 41 deletions.
diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h
@@ -28,23 +28,115 @@ namespace tmx
             static constexpr const char *MessageSubType = MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING;
 
             // TODO: Convert this member variable to std::attributes and handle nested object and arrays. (see [CloudHeartbeatMessage.h](./CloudHearbeatMessage.h) array_attribute )
-
-            // Classification of detected object
-            std::string type = "";
+            //Flag to indicate whether sensor detected object is simulated.
+            std_attribute(this->msg, bool, ISSimulated, false,);
+            // Classification of detected object.
+            std_attribute(this->msg, std::string, Type, "",);
             // Confidence of type classification
-            double confidence = 0.0;
-            // Unique indentifier of sensor reporting detection
-            std::string sensorId = "";
-            // String describing projection used to convert cartesian data to WGS84 data 
-            std::string projString = "";
-            // Unique identifier of detected object
-            int objectId = 0;
+            std_attribute(this->msg, double, Confidence, 0.0,);
+            // Unique indentifier of sensor reporting detection.
+            std_attribute(this->msg, std::string, SensorId, "", );
+            // String describing projection used to convert cartesian data to WGS84 data.
+            std_attribute(this->msg, std::string, ProjString, "", );
+            // Unique identifier of detected object.
+            std_attribute(this->msg, int, ObjectId, 0, );
+
             // Cartesian positiion of object. Assumed to be ENU coordinate frame.
-            tmx::utils::Point position = tmx::utils::Point();
+            typedef struct Position{
+                double x,  y, z;
+                Position(){};
+                Position(double x, double y, double z):x(x),y(y),z(z){};
+                static message_tree_type to_tree(const Position& pos){
+                    message_tree_type tree;
+                    tree.put("x", pos.x);
+                    tree.put("y", pos.y);
+                    tree.put("z", pos.z);
+                    return tree;
+                }
+                static Position from_tree(const message_tree_type& tree){
+                    Position pos;
+                    pos.x = tree.get<double>("x");
+                    pos.y = tree.get<double>("y");
+                    pos.z = tree.get<double>("z");
+                    return pos;
+                }
+            } Position;            
+            object_attribute(Position, Position);
+
+            typedef struct Covariance{
+                std::string value;                
+                Covariance(){};
+                Covariance( std::string value):value(value){};
+                static message_tree_type to_tree(const Covariance& cov){
+                    message_tree_type tree;
+                    tree.put("", cov.value);
+                    return tree;
+                }
+                static Covariance from_tree(const message_tree_type& tree){
+                    Covariance cov;
+                    cov.value = tree.get<std::string>("");
+                    return cov;
+                }
+            } Covariance;
+            array_attribute(Covariance, PositionCovariance);
+
             // Cartesian velocity vector of object. Assumed to be ENU coordinate frame.
-            tmx::utils::Vector3d velocity = tmx::utils::Vector3d();
-            // Epoch time in milliseconds
-            long timestamp = 0;
+            typedef struct Velocity{
+                double x, y, z;
+                Velocity(){};
+                Velocity(double x, double y, double z):x(x),y(y),z(z){};
+                static message_tree_type to_tree(const Velocity& velocity){
+                    message_tree_type tree;
+                    tree.put("x", velocity.x);
+                    tree.put("y", velocity.y);
+                    tree.put("z", velocity.z);
+                    return tree;
+                }
+                static Velocity from_tree(const message_tree_type& tree){
+                    Velocity velocity;
+                    velocity.x = tree.get<double>("x");
+                    velocity.y = tree.get<double>("y");
+                    velocity.z = tree.get<double>("z");
+                    return velocity;
+                }
+            } Velocity;
+            //Linear velocity in meter per second
+            object_attribute(Velocity, Velocity);
+            //Covariance associated with linear velocity.
+            array_attribute(Covariance, VelocityCovariance);
+
+            //Angular velocity in radians per second.
+            object_attribute(Velocity, AngularVelocity);
+            //Covariance associated with angular velocity.
+            array_attribute(Covariance, AngularVelocityCovariance);
+
+            // Epoch time in milliseconds.
+            // long timestamp = 0;
+            std_attribute(this->msg, long, Timestamp, 0, );
+
+            //Length, width and height of object in meter.
+            typedef struct Size{
+                double length;
+                double width;
+                double height;
+                Size(){};
+                Size(double length, double width, double height): length(length), width(width), height(height){};
+                static message_tree_type to_tree(const Size& size){
+                    message_tree_type tree;
+                    tree.put("length", size.length);
+                    tree.put("width", size.width);
+                    tree.put("height", size.height);
+                    return tree;
+                }
+                static Size from_tree(const message_tree_type & tree){
+                    Size size;
+                    size.length = tree.get<double>("length");
+                    size.width = tree.get<double>("width");
+                    size.height = tree.get<double>("height");
+                    return size;
+                }
+            } Size;
+            object_attribute(Size, Size);
 
         };
 

diff --git a/src/tmx/TmxApi/tmx/messages/message.hpp b/src/tmx/TmxApi/tmx/messages/message.hpp
@@ -22,6 +22,11 @@
 	void add_to_##NAME(ELEMENT element) { add_array_element<ELEMENT>(#NAME, element); } \
 	void erase_##NAME()	{ erase_array(#NAME); }
 
+#define object_attribute(ELEMENT, NAME) \
+	ELEMENT get_##NAME() {return get_object<ELEMENT>(#NAME); } \
+	void set_##NAME(ELEMENT obj) {return set_object<ELEMENT>(#NAME, obj); } \
+	void erase_##NAME(){erase_object(#NAME); }
+
 namespace tmx
 {
 
@@ -402,6 +407,34 @@ class tmx_message {
 		tree.get().push_back(typename message_tree_type::value_type("", Element::to_tree(element)));
 	}
 
+	/***
+	 * @brief Get the content of an object fields
+	 * @param Name of the object
+	 * @param Object An object containing all the fields
+	 */
+	template <class Element>
+	Element get_object(const std::string&  objectName){
+		boost::optional<boost::property_tree::ptree &> tree = this->as_tree();
+		return Element::from_tree(tree.get().get_child(objectName));
+	}
+
+	/**
+	 * @brief Set the content of an object fields
+	 * @param Name of the object 
+	 * @param Object An object containing all the fields to set
+	 */
+	template <class Element>
+	void set_object(const std::string&  objectName, Element obj)
+	{
+		erase_object(objectName);
+		this->as_tree().get().add_child(objectName, Element::to_tree(obj));
+	}
+
+	void erase_object(const std::string& objName)
+	{
+		this->as_tree().get().erase(objName);
+	}
+
 	/**
 	 * Erase the entire contents of an array field.
 	 * @param The name of the array field.

diff --git a/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp b/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp
@@ -0,0 +1,52 @@
+#include <gtest/gtest.h>
+#include "SensorDetectedObject.h"
+namespace tmx::messages{
+    TEST(SensorDetectedObjectTest, attributes){
+        SensorDetectedObject tmxSdsm;
+        tmxSdsm.set_ISSimulated(false);
+        SensorDetectedObject::Position pos(1.0, 2.3, 2.0);
+        tmxSdsm.set_Position(pos);
+
+        tmxSdsm.set_ProjString("+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs");
+        tmxSdsm.set_Timestamp(12222222222);
+        tmxSdsm.set_SensorId("SomeID");
+        tmxSdsm.set_Type("Car");
+        tmxSdsm.set_Confidence(0.7);
+        tmxSdsm.set_ObjectId(123);
+
+        SensorDetectedObject::Velocity vel(1.0, 0.3, 2.0);
+        tmxSdsm.set_Velocity(vel);
+        tmxSdsm.set_AngularVelocity(vel);
+
+        std::vector<SensorDetectedObject::Covariance> covs { 
+            SensorDetectedObject::Covariance("a11"), 
+            SensorDetectedObject::Covariance("a12"),
+            SensorDetectedObject::Covariance("a13"), 
+            SensorDetectedObject::Covariance("a21"),
+            SensorDetectedObject::Covariance("a22"), 
+            SensorDetectedObject::Covariance("a23"),
+            SensorDetectedObject::Covariance("a31"), 
+            SensorDetectedObject::Covariance("a32"),
+            SensorDetectedObject::Covariance("a33"), 
+            SensorDetectedObject::Covariance("a41")};
+        tmxSdsm.set_PositionCovariance(covs);
+        tmxSdsm.set_VelocityCovariance(covs);
+        tmxSdsm.set_AngularVelocityCovariance(covs);
+
+        EXPECT_EQ(false, tmxSdsm.get_ISSimulated());
+        EXPECT_EQ(0.7, tmxSdsm.get_Confidence());
+        EXPECT_EQ("SomeID", tmxSdsm.get_SensorId());
+        EXPECT_EQ(12222222222, tmxSdsm.get_Timestamp());
+        EXPECT_EQ(123, tmxSdsm.get_ObjectId());
+        EXPECT_EQ("+proj=tmerc +lat_0=38.95197911150576 +lon_0=-77.14835128349988 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs", tmxSdsm.get_ProjString());
+        EXPECT_EQ(1.0, tmxSdsm.get_Velocity().x);
+        EXPECT_NEAR(0.3, tmxSdsm.get_Velocity().y, 0.01);
+        EXPECT_EQ(2.0, tmxSdsm.get_Velocity().z);
+        EXPECT_EQ(1.0, tmxSdsm.get_AngularVelocity().x);
+        EXPECT_NEAR(0.3, tmxSdsm.get_AngularVelocity().y, 0.01);
+        EXPECT_EQ(2.0, tmxSdsm.get_AngularVelocity().z);
+        EXPECT_EQ(10, tmxSdsm.get_PositionCovariance().size());
+        EXPECT_EQ(10, tmxSdsm.get_AngularVelocityCovariance().size());
+        EXPECT_EQ(10, tmxSdsm.get_VelocityCovariance().size());
+    }
+}
diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp
@@ -31,15 +31,15 @@ namespace MUSTSensorDriverPlugin {
 
     tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view 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 = static_cast<long>(detection.timestamp*1000); // convert decimal seconds to int milliseconds.
-        detectedObject.velocity = headingSpeedToVelocity(detection.heading, detection.speed);
-        detectedObject.type = detectionClassificationToSensorDetectedObjectType(detection.cl);
-        detectedObject.sensorId = sensorId;
-        detectedObject.projString = projString;
+        detectedObject.set_ObjectId(detection.trackID);
+        tmx::messages::SensorDetectedObject::Position pos(detection.position_x, detection.position_y, 0);
+        detectedObject.set_Position(pos);
+        detectedObject.set_Confidence(detection.confidence);
+        detectedObject.set_Timestamp(static_cast<long>(detection.timestamp*1000)); // convert decimal seconds to int milliseconds.
+        detectedObject.set_Velocity(headingSpeedToVelocity(detection.heading, detection.speed));
+        detectedObject.set_Type(detectionClassificationToSensorDetectedObjectType(detection.cl));
+        detectedObject.set_SensorId(std::string(sensorId));
+        detectedObject.set_ProjString(std::string(projString));
         return detectedObject;
     }
     DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept {
@@ -77,15 +77,15 @@ namespace MUSTSensorDriverPlugin {
         }
     };
 
-    tmx::utils::Vector3d headingSpeedToVelocity(double heading, double speed) {
+    tmx::messages::SensorDetectedObject::Velocity headingSpeedToVelocity(double heading, double speed) {
         // 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(headingInRad * heading) * speed;
-        velocity.Y = std::sin(headingInRad * heading) * speed;
-        velocity.Z = 0;
+        tmx::messages::SensorDetectedObject::Velocity velocity;
+        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
@@ -118,5 +118,5 @@ namespace MUSTSensorDriverPlugin {
      * @param speed double speed in m/s
      * @return tmx::utils::Vector3d velocity.
      */
-    tmx::utils::Vector3d headingSpeedToVelocity(double heading, double speed);
+    tmx::messages::SensorDetectedObject::Velocity headingSpeedToVelocity(double heading, double speed);
 }
diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp
@@ -62,16 +62,16 @@ TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) {
 
     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(1719506355400, sensorDetectedObject.timestamp);
-    EXPECT_EQ("MUSTSensor1", sensorDetectedObject.sensorId);
-    EXPECT_EQ("PROJ String", sensorDetectedObject.projString);
+    EXPECT_EQ(detection.trackID, sensorDetectedObject.get_ObjectId());
+    EXPECT_DOUBLE_EQ(detection.confidence, sensorDetectedObject.get_Confidence());
+    EXPECT_DOUBLE_EQ(detection.position_x, sensorDetectedObject.get_Position().x);
+    EXPECT_DOUBLE_EQ(detection.position_y, sensorDetectedObject.get_Position().y);
+    EXPECT_NEAR(4.33, sensorDetectedObject.get_Velocity().y, 0.001);
+    EXPECT_NEAR(2.5, sensorDetectedObject.get_Velocity().x, 0.001);
+    EXPECT_STRCASEEQ("SEDAN", sensorDetectedObject.get_Type().c_str());
+    EXPECT_EQ(1719506355400, sensorDetectedObject.get_Timestamp());
+    EXPECT_EQ("MUSTSensor1", sensorDetectedObject.get_SensorId());
+    EXPECT_EQ("PROJ String", sensorDetectedObject.get_ProjString());
 }
 
 TEST(TestMUSTSensorDetection, detectionClassificationToSensorDetectedObjectType ) {
@@ -84,6 +84,6 @@ TEST(TestMUSTSensorDetection, detectionClassificationToSensorDetectedObjectType
 
 TEST(TestMUSTSensorDetection, headingSpeedToVelocity ) {
     auto velocity = headingSpeedToVelocity(30, 5);
-    EXPECT_NEAR(4.33, velocity.Y, 0.001);
-    EXPECT_NEAR(-2.5, velocity.X, 0.001);
+    EXPECT_NEAR(4.33, velocity.y, 0.001);
+    EXPECT_NEAR(-2.5, velocity.x, 0.001);
 }