FCP-5: SensorDetectedObject TMX message definition

src/tmx/Messages/include/Covariance.h

@@ -0,0 +1,23 @@
+#pragma once
+#include <tmx/messages/message.hpp>
+namespace tmx
+{
+    namespace messages
+    {
+        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;   
+    }
+}

src/tmx/Messages/include/Position.h

@@ -0,0 +1,28 @@
+#pragma once
+#include <tmx/messages/message.hpp>
+namespace tmx
+{
+    namespace messages
+    {
+        // Cartesian positiion of object. Assumed to be ENU coordinate frame.
+        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;       
+    }
+}

src/tmx/Messages/include/SensorDetectedObject.h

@@ -5,6 +5,10 @@
 #include <MessageTypes.h>
 #include <Vector3d.h>
 #include <Point.h>
+#include "Position.h"
+#include "Covariance.h"
+#include "Velocity.h"
+#include "Size.h"
 
 namespace tmx
 {
@@ -28,23 +32,35 @@ 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;
-            // Cartesian positiion of object. Assumed to be ENU coordinate frame.
-            tmx::utils::Point position = tmx::utils::Point();
-            // 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;
+            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, );
+
+
+            object_attribute(Position, Position);           
+            array_attribute(Covariance, PositionCovariance);
+            //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, );            
+            object_attribute(Size, Size);
 
         };
 

src/tmx/Messages/include/Size.h

@@ -0,0 +1,30 @@
+#pragma once
+#include <tmx/messages/message.hpp>
+namespace tmx
+{
+    namespace messages
+    {
+        //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;
+    }
+}

src/tmx/Messages/include/Velocity.h

@@ -0,0 +1,28 @@
+#pragma once
+#include <tmx/messages/message.hpp>
+namespace tmx
+{
+    namespace messages
+    {
+       // Cartesian velocity vector of object. Assumed to be ENU coordinate frame.
+        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; 
+    }
+}

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.

src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp

@@ -0,0 +1,79 @@
+#include <gtest/gtest.h>
+#include "SensorDetectedObject.h"
+
+namespace tmx::messages{
+    class SensorDetectedObjectTest : public testing::Test{
+        protected:
+                std::shared_ptr<SensorDetectedObject> tmxSdsmPtr;   
+        SensorDetectedObjectTest(){
+            tmxSdsmPtr = std::make_shared<SensorDetectedObject>();
+        }         
+        void SetUp() override {
+            tmxSdsmPtr->set_ISSimulated(false);
+            Position pos(1.0, 2.3, 2.0);
+            tmxSdsmPtr->set_Position(pos);
+            tmxSdsmPtr->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");
+            tmxSdsmPtr->set_Timestamp(12222222222);
+            tmxSdsmPtr->set_SensorId("SomeID");
+            tmxSdsmPtr->set_Type("Car");
+            tmxSdsmPtr->set_Confidence(0.7);
+            tmxSdsmPtr->set_ObjectId(123);
+
+            Velocity vel(1.0, 0.3, 2.0);
+            tmxSdsmPtr->set_Velocity(vel);
+            tmxSdsmPtr->set_AngularVelocity(vel);
+
+            std::vector<Covariance> covs { 
+                Covariance("a11"), 
+                Covariance("a12"),
+                Covariance("a13"), 
+                Covariance("a21"),
+                Covariance("a22"), 
+                Covariance("a23"),
+                Covariance("a31"), 
+                Covariance("a32"),
+                Covariance("a33"), 
+                Covariance("a41")};
+            tmxSdsmPtr->set_PositionCovariance(covs);
+            tmxSdsmPtr->set_VelocityCovariance(covs);
+            tmxSdsmPtr->set_AngularVelocityCovariance(covs);
+        }
+    };
+
+    TEST_F(SensorDetectedObjectTest, attributes){    
+        EXPECT_EQ(false, tmxSdsmPtr->get_ISSimulated());
+        EXPECT_EQ(0.7, tmxSdsmPtr->get_Confidence());
+        EXPECT_EQ("SomeID", tmxSdsmPtr->get_SensorId());
+        EXPECT_EQ(12222222222, tmxSdsmPtr->get_Timestamp());
+        EXPECT_EQ(123, tmxSdsmPtr->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", tmxSdsmPtr->get_ProjString());
+        EXPECT_EQ(1.0, tmxSdsmPtr->get_Position().x);
+        EXPECT_NEAR(2.3, tmxSdsmPtr->get_Position().y, 0.01);
+        EXPECT_EQ(2.0, tmxSdsmPtr->get_Position().z);
+        EXPECT_EQ(1.0, tmxSdsmPtr->get_Velocity().x);
+        EXPECT_NEAR(0.3, tmxSdsmPtr->get_Velocity().y, 0.01);
+        EXPECT_EQ(2.0, tmxSdsmPtr->get_Velocity().z);
+        EXPECT_EQ(1.0, tmxSdsmPtr->get_AngularVelocity().x);
+        EXPECT_NEAR(0.3, tmxSdsmPtr->get_AngularVelocity().y, 0.01);
+        EXPECT_EQ(2.0, tmxSdsmPtr->get_AngularVelocity().z);
+        EXPECT_EQ(10, tmxSdsmPtr->get_PositionCovariance().size());
+        EXPECT_EQ(10, tmxSdsmPtr->get_AngularVelocityCovariance().size());
+        EXPECT_EQ(10, tmxSdsmPtr->get_VelocityCovariance().size());
+    }   
+
+    TEST_F(SensorDetectedObjectTest, to_string){
+        std::string expectedStr = "{\"ISSimulated\":\"0\",\"Position\":{\"x\":\"1\",\"y\":\"2.2999999999999998\",\"z\":\"2\"},\"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\",\"Timestamp\":\"12222222222\",\"SensorId\":\"SomeID\",\"Type\":\"Car\",\"Confidence\":\"0.69999999999999996\",\"ObjectId\":\"123\",\"Velocity\":{\"x\":\"1\",\"y\":\"0.29999999999999999\",\"z\":\"2\"},\"AngularVelocity\":{\"x\":\"1\",\"y\":\"0.29999999999999999\",\"z\":\"2\"},\"PositionCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"],\"VelocityCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"],\"AngularVelocityCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"]}\n";
+        EXPECT_EQ(expectedStr, tmxSdsmPtr->to_string());
+    }
+
+    TEST_F(SensorDetectedObjectTest, deserialize){
+        auto tmxSdsmPtr2 = std::make_shared<SensorDetectedObject>();
+        std::string expectedStr = "{\"ISSimulated\":\"1\",\"Position\":{\"x\":\"1\",\"y\":\"2.5\",\"z\":\"2\"},\"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\",\"Timestamp\":\"12222222222\",\"SensorId\":\"SomeID\",\"Type\":\"Car\",\"Confidence\":\"0.7\",\"ObjectId\":\"123\",\"Velocity\":{\"x\":\"1\",\"y\":\"0.5\",\"z\":\"2\"},\"AngularVelocity\":{\"x\":\"1\",\"y\":\"0.3\",\"z\":\"2\"},\"PositionCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"],\"VelocityCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"],\"AngularVelocityCovariance\":[\"a11\",\"a12\",\"a13\",\"a21\",\"a22\",\"a23\",\"a31\",\"a32\",\"a33\",\"a41\"]}\n";
+        tmxSdsmPtr2->set_contents(expectedStr);
+        EXPECT_EQ(expectedStr, tmxSdsmPtr2->to_string());
+        EXPECT_EQ(true, tmxSdsmPtr2->get_ISSimulated());
+        EXPECT_EQ(1.0, tmxSdsmPtr2->get_Position().x);
+        EXPECT_NEAR(2.5, tmxSdsmPtr2->get_Position().y, 0.01);
+        EXPECT_EQ(2.0, tmxSdsmPtr2->get_Position().z);
+    }
+}

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::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::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::Velocity velocity;
+        velocity.x = std::cos(headingInRad * heading) * speed;
+        velocity.y = std::sin(headingInRad * heading) * speed;
+        velocity.z = 0;
         return velocity;
     };
 }

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::Velocity headingSpeedToVelocity(double heading, double speed);
 }

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);
 }