From 78f4bb604ef599cd4f1ac314659b5e4856f09996 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 6 Aug 2024 22:25:21 +0000 Subject: [PATCH] init --- .../Messages/include/SensorDetectedObject.h | 120 ++++++++++++++++-- src/tmx/TmxApi/tmx/messages/message.hpp | 33 +++++ .../test/SensorDetectedObjectTest.cpp | 52 ++++++++ .../src/MUSTSensorDetection.cpp | 28 ++-- .../src/MUSTSensorDetection.h | 2 +- .../test/TestMUSTSensorDetection.cpp | 24 ++-- 6 files changed, 218 insertions(+), 41 deletions(-) create mode 100644 src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h index d275bbf7e..ec15cd7d1 100644 --- 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("x"); + pos.y = tree.get("y"); + pos.z = tree.get("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(""); + 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("x"); + velocity.y = tree.get("y"); + velocity.z = tree.get("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("length"); + size.width = tree.get("width"); + size.height = tree.get("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 index 18570d558..a82950abc 100644 --- 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(#NAME, element); } \ void erase_##NAME() { erase_array(#NAME); } +#define object_attribute(ELEMENT, NAME) \ + ELEMENT get_##NAME() {return get_object(#NAME); } \ + void set_##NAME(ELEMENT obj) {return set_object(#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 + Element get_object(const std::string& objectName){ + boost::optional 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 + 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 new file mode 100644 index 000000000..511817be8 --- /dev/null +++ b/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp @@ -0,0 +1,52 @@ +#include +#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 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()); + } +} \ 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 95b514d9e..97a28057c 100644 --- 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(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(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 index 313204811..b4f770642 100644 --- 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); } \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp index a50783d3a..0d8fb0e22 100644 --- 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); } \ No newline at end of file