From 78f4bb604ef599cd4f1ac314659b5e4856f09996 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 6 Aug 2024 22:25:21 +0000 Subject: [PATCH 01/13] 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 From 391433049b30ac825d763f49b05c2d701e62e0d6 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 12 Aug 2024 14:35:45 +0000 Subject: [PATCH 02/13] address comments --- src/tmx/Messages/include/Covariance.h | 23 ++++ src/tmx/Messages/include/Position.h | 28 +++++ .../Messages/include/SensorDetectedObject.h | 90 ++------------ src/tmx/Messages/include/Size.h | 30 +++++ src/tmx/Messages/include/Velocity.h | 28 +++++ .../test/SensorDetectedObjectTest.cpp | 113 +++++++++++------- .../src/MUSTSensorDetection.cpp | 6 +- .../src/MUSTSensorDetection.h | 2 +- 8 files changed, 190 insertions(+), 130 deletions(-) create mode 100644 src/tmx/Messages/include/Covariance.h create mode 100644 src/tmx/Messages/include/Position.h create mode 100644 src/tmx/Messages/include/Size.h create mode 100644 src/tmx/Messages/include/Velocity.h diff --git a/src/tmx/Messages/include/Covariance.h b/src/tmx/Messages/include/Covariance.h new file mode 100644 index 000000000..711548d85 --- /dev/null +++ b/src/tmx/Messages/include/Covariance.h @@ -0,0 +1,23 @@ +#pragma once +#include +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(""); + return cov; + } + } Covariance; + } +} \ No newline at end of file diff --git a/src/tmx/Messages/include/Position.h b/src/tmx/Messages/include/Position.h new file mode 100644 index 000000000..22f8e8501 --- /dev/null +++ b/src/tmx/Messages/include/Position.h @@ -0,0 +1,28 @@ +#pragma once +#include +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("x"); + pos.y = tree.get("y"); + pos.z = tree.get("z"); + return pos; + } + } Position; + } +} \ No newline at end of file diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h index ec15cd7d1..a564e6bc6 100644 --- a/src/tmx/Messages/include/SensorDetectedObject.h +++ b/src/tmx/Messages/include/SensorDetectedObject.h @@ -5,6 +5,10 @@ #include #include #include +#include "Position.h" +#include "Covariance.h" +#include "Velocity.h" +#include "Size.h" namespace tmx { @@ -41,70 +45,13 @@ namespace tmx // Unique identifier of detected object. std_attribute(this->msg, int, ObjectId, 0, ); - // 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("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; + + object_attribute(Position, Position); array_attribute(Covariance, PositionCovariance); - - // 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("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. @@ -112,30 +59,7 @@ namespace tmx // 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; + std_attribute(this->msg, long, Timestamp, 0, ); object_attribute(Size, Size); }; diff --git a/src/tmx/Messages/include/Size.h b/src/tmx/Messages/include/Size.h new file mode 100644 index 000000000..7953ccbe8 --- /dev/null +++ b/src/tmx/Messages/include/Size.h @@ -0,0 +1,30 @@ +#pragma once +#include +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("length"); + size.width = tree.get("width"); + size.height = tree.get("height"); + return size; + } + } Size; + } +} \ No newline at end of file diff --git a/src/tmx/Messages/include/Velocity.h b/src/tmx/Messages/include/Velocity.h new file mode 100644 index 000000000..28feb4b25 --- /dev/null +++ b/src/tmx/Messages/include/Velocity.h @@ -0,0 +1,28 @@ +#pragma once +#include +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("x"); + velocity.y = tree.get("y"); + velocity.z = tree.get("z"); + return velocity; + } + } Velocity; + } +} \ No newline at end of file diff --git a/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp b/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp index 511817be8..e2875396e 100644 --- a/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp +++ b/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp @@ -1,52 +1,79 @@ #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); + class SensorDetectedObjectTest : public testing::Test{ + protected: + std::shared_ptr tmxSdsmPtr; + SensorDetectedObjectTest(){ + tmxSdsmPtr = std::make_shared(); + } + 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); - 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); + Velocity vel(1.0, 0.3, 2.0); + tmxSdsmPtr->set_Velocity(vel); + tmxSdsmPtr->set_AngularVelocity(vel); - SensorDetectedObject::Velocity vel(1.0, 0.3, 2.0); - tmxSdsm.set_Velocity(vel); - tmxSdsm.set_AngularVelocity(vel); + std::vector 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); + } + }; - 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); + 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()); + } - 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()); + TEST_F(SensorDetectedObjectTest, deserialize){ + auto tmxSdsmPtr2 = std::make_shared(); + 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); } } \ 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 97a28057c..b193f82b8 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp @@ -32,7 +32,7 @@ namespace MUSTSensorDriverPlugin { tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString) { tmx::messages::SensorDetectedObject detectedObject; detectedObject.set_ObjectId(detection.trackID); - tmx::messages::SensorDetectedObject::Position pos(detection.position_x, detection.position_y, 0); + 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(detection.timestamp*1000)); // convert decimal seconds to int milliseconds. @@ -77,12 +77,12 @@ namespace MUSTSensorDriverPlugin { } }; - tmx::messages::SensorDetectedObject::Velocity 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::messages::SensorDetectedObject::Velocity velocity; + tmx::messages::Velocity velocity; velocity.x = std::cos(headingInRad * heading) * speed; velocity.y = std::sin(headingInRad * heading) * speed; velocity.z = 0; diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h index b4f770642..37e0858ec 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::messages::SensorDetectedObject::Velocity headingSpeedToVelocity(double heading, double speed); + tmx::messages::Velocity headingSpeedToVelocity(double heading, double speed); } \ No newline at end of file From c0a88382ccad45cfd734dfd82cf5101161242934 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 12 Aug 2024 16:22:08 +0000 Subject: [PATCH 03/13] add comment --- src/tmx/TmxApi/tmx/messages/message.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/tmx/TmxApi/tmx/messages/message.hpp b/src/tmx/TmxApi/tmx/messages/message.hpp index a82950abc..3ac3960cf 100644 --- a/src/tmx/TmxApi/tmx/messages/message.hpp +++ b/src/tmx/TmxApi/tmx/messages/message.hpp @@ -429,7 +429,11 @@ class tmx_message { erase_object(objectName); this->as_tree().get().add_child(objectName, Element::to_tree(obj)); } - + /** + * @brief Erase a certain object from the tree given the object name. + * @param Name of the object + * @param Object An object to be erased from the tree + */ void erase_object(const std::string& objName) { this->as_tree().get().erase(objName); From 14de679dd03f4d494e2f757cddb0223aff561c7b Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 12 Aug 2024 20:58:37 +0000 Subject: [PATCH 04/13] add nested array --- src/tmx/Messages/CMakeLists.txt | 15 +++ src/tmx/Messages/include/Covariance.h | 8 +- .../Messages/include/SensorDetectedObject.h | 28 ++--- .../test/SensorDetectedObjectTest.cpp | 108 ++++++++++++++++++ src/tmx/Messages/test/TestTimeSyncMessage.cpp | 2 +- src/tmx/TmxApi/tmx/messages/message.hpp | 64 +++++++++++ .../test/SensorDetectedObjectTest.cpp | 79 ------------- 7 files changed, 206 insertions(+), 98 deletions(-) create mode 100644 src/tmx/Messages/test/SensorDetectedObjectTest.cpp delete mode 100644 src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp diff --git a/src/tmx/Messages/CMakeLists.txt b/src/tmx/Messages/CMakeLists.txt index 3b866ee13..89d08688d 100644 --- a/src/tmx/Messages/CMakeLists.txt +++ b/src/tmx/Messages/CMakeLists.txt @@ -5,3 +5,18 @@ SET (TMXMESSAGES_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/include" PARENT_SCOPE) INSTALL (DIRECTORY include DESTINATION . COMPONENT ${PROJECT_NAME} FILES_MATCHING PATTERN "*.h*") + +############# +## Testing ## +############# +enable_testing() + +set(BINARY ${PROJECT_NAME}_test) + +file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) + +add_executable(${BINARY} ${TEST_SOURCES}) + +add_test(NAME ${BINARY} COMMAND ${BINARY}) + +target_link_libraries(${BINARY} PUBLIC ${TMXAPI_LIBRARIES} gtest) \ No newline at end of file diff --git a/src/tmx/Messages/include/Covariance.h b/src/tmx/Messages/include/Covariance.h index 711548d85..1259824a3 100644 --- a/src/tmx/Messages/include/Covariance.h +++ b/src/tmx/Messages/include/Covariance.h @@ -5,17 +5,17 @@ namespace tmx namespace messages { typedef struct Covariance{ - std::string value; + double value; Covariance(){}; - Covariance( std::string value):value(value){}; + Covariance(double value):value(value){}; static message_tree_type to_tree(const Covariance& cov){ message_tree_type tree; - tree.put("", cov.value); + tree.put("",cov.value); return tree; } static Covariance from_tree(const message_tree_type& tree){ Covariance cov; - cov.value = tree.get(""); + cov.value = tree.get(""); return cov; } } Covariance; diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h index a564e6bc6..504eb910d 100644 --- a/src/tmx/Messages/include/SensorDetectedObject.h +++ b/src/tmx/Messages/include/SensorDetectedObject.h @@ -33,34 +33,34 @@ namespace tmx // TODO: Convert this member variable to std::attributes and handle nested object and arrays. (see [CloudHeartbeatMessage.h](./CloudHearbeatMessage.h) array_attribute ) //Flag to indicate whether sensor detected object is simulated. - std_attribute(this->msg, bool, ISSimulated, false,); + std_attribute(this->msg, bool, isSimulated, false,); // Classification of detected object. - std_attribute(this->msg, std::string, Type, "",); + std_attribute(this->msg, std::string, type, "",); // Confidence of type classification - std_attribute(this->msg, double, Confidence, 0.0,); + std_attribute(this->msg, double, confidence, 0.0,); // Unique indentifier of sensor reporting detection. - std_attribute(this->msg, std::string, SensorId, "", ); + 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, "", ); + std_attribute(this->msg, std::string, projString, "", ); // Unique identifier of detected object. - std_attribute(this->msg, int, ObjectId, 0, ); + std_attribute(this->msg, int, objectId, 0, ); - object_attribute(Position, Position); - array_attribute(Covariance, PositionCovariance); + object_attribute(Position, position); + two_dimension_array_attribute(Covariance, positionCovariance); //Linear velocity in meter per second - object_attribute(Velocity, Velocity); + object_attribute(Velocity, velocity); //Covariance associated with linear velocity. - array_attribute(Covariance, VelocityCovariance); + two_dimension_array_attribute(Covariance, velocityCovariance); //Angular velocity in radians per second. - object_attribute(Velocity, AngularVelocity); + object_attribute(Velocity, angularVelocity); //Covariance associated with angular velocity. - array_attribute(Covariance, AngularVelocityCovariance); + two_dimension_array_attribute(Covariance, angularVelocityCovariance); // Epoch time in milliseconds. // long timestamp = 0; - std_attribute(this->msg, long, Timestamp, 0, ); - object_attribute(Size, Size); + std_attribute(this->msg, long, timestamp, 0, ); + object_attribute(Size, size); }; diff --git a/src/tmx/Messages/test/SensorDetectedObjectTest.cpp b/src/tmx/Messages/test/SensorDetectedObjectTest.cpp new file mode 100644 index 000000000..279cbcf26 --- /dev/null +++ b/src/tmx/Messages/test/SensorDetectedObjectTest.cpp @@ -0,0 +1,108 @@ +#include +#include "SensorDetectedObject.h" + +namespace tmx::messages{ + class SensorDetectedObjectTest : public testing::Test{ + protected: + std::shared_ptr tmxSdsmPtr; + SensorDetectedObjectTest(){ + tmxSdsmPtr = std::make_shared(); + } + 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 covs { + Covariance(12), + Covariance(11), + Covariance(13), + Covariance(14), + Covariance(15) + }; + int covarianceSize = 3; + std::vector> covs2d; + for(int i=0; iset_positionCovariance(covs2d); + tmxSdsmPtr->set_velocityCovariance(covs2d); + tmxSdsmPtr->set_angularVelocityCovariance(covs2d); + } + }; + + 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(3, tmxSdsmPtr->get_positionCovariance().size()); + EXPECT_EQ(3, tmxSdsmPtr->get_angularVelocityCovariance().size()); + EXPECT_EQ(3, 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\":[[\"12\",\"11\",\"13\",\"14\",\"15\"],[\"12\",\"11\",\"13\",\"14\",\"15\"],[\"12\",\"11\",\"13\",\"14\",\"15\"]],\"velocityCovariance\":[[\"12\",\"11\",\"13\",\"14\",\"15\"],[\"12\",\"11\",\"13\",\"14\",\"15\"],[\"12\",\"11\",\"13\",\"14\",\"15\"]],\"angularVelocityCovariance\":[[\"12\",\"11\",\"13\",\"14\",\"15\"],[\"12\",\"11\",\"13\",\"14\",\"15\"],[\"12\",\"11\",\"13\",\"14\",\"15\"]]}\n"; + EXPECT_EQ(expectedStr, tmxSdsmPtr->to_string()); + } + + TEST_F(SensorDetectedObjectTest, deserialize){ + auto tmxSdsmPtr2 = std::make_shared(); + std::string expectedStr = "{\"isSimulated\":1,\"type\":\"CAR\",\"confidence\":1.0,\"sensorId\":\"IntersectionLidar\",\"projString\":\"+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\",\"objectId\":207,\"position\":{\"x\":-5.021,\"y\":64.234,\"z\":-10.297},\"positionCovariance\":[[0.04000000000000001,0.0,0.0],[0.0,0.04000000000000001,0.0],[0.0,0.0,0.04000000000000001]],\"velocity\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"velocityCovariance\":[[0.04000000000000001,0.0,0.0],[0.0,0.04000000000000001,0.0],[0.0,0.0,0.04000000000000001]],\"angularVelocity\":{\"x\":0.0,\"y\":-0.0,\"z\":-0.0},\"angularVelocityCovariance\":[[0.010000000000000002,0.0,0.0],[0.0,0.010000000000000002,0.0],[0.0,0.0,0.010000000000000002]],\"size\":{\"length\":2.257,\"height\":1.003,\"width\":0.762},\"timestamp\":110200}"; + tmxSdsmPtr2->set_contents(expectedStr); + EXPECT_EQ(expectedStr, tmxSdsmPtr2->to_string()); + EXPECT_EQ(true, tmxSdsmPtr2->get_isSimulated()); + EXPECT_EQ(-5.021, tmxSdsmPtr2->get_position().x); + EXPECT_NEAR(64.234, tmxSdsmPtr2->get_position().y, 0.01); + EXPECT_EQ(-10.297, tmxSdsmPtr2->get_position().z); + EXPECT_EQ("CAR", tmxSdsmPtr2->get_type()); + EXPECT_EQ(1.0, tmxSdsmPtr2->get_confidence()); + EXPECT_EQ("IntersectionLidar", tmxSdsmPtr2->get_sensorId()); + EXPECT_EQ("+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", tmxSdsmPtr2->get_projString()); + EXPECT_EQ(207, tmxSdsmPtr2->get_objectId()); + EXPECT_EQ(0.0, tmxSdsmPtr2->get_velocity().x); + EXPECT_EQ(0.0, tmxSdsmPtr2->get_velocity().y); + EXPECT_EQ(0.0, tmxSdsmPtr2->get_velocity().z); + EXPECT_EQ(0.0, tmxSdsmPtr2->get_angularVelocity().x); + EXPECT_EQ(-0.0, tmxSdsmPtr2->get_angularVelocity().y); + EXPECT_EQ(-0.0, tmxSdsmPtr2->get_angularVelocity().z); + EXPECT_EQ(2.257, tmxSdsmPtr2->get_size().length); + EXPECT_EQ(1.003, tmxSdsmPtr2->get_size().height); + EXPECT_EQ(0.762, tmxSdsmPtr2->get_size().width); + EXPECT_EQ(110200, tmxSdsmPtr2->get_timestamp()); + + EXPECT_EQ(3, tmxSdsmPtr2->get_positionCovariance().size()); + EXPECT_EQ(3, tmxSdsmPtr2->get_angularVelocityCovariance().size()); + EXPECT_EQ(3, tmxSdsmPtr2->get_velocityCovariance().size()); + + EXPECT_NEAR(0.04,tmxSdsmPtr2->get_positionCovariance().begin()->begin()->value, 0.0001); + EXPECT_EQ(0.0, tmxSdsmPtr2->get_positionCovariance().begin()->back().value); + + EXPECT_NEAR(0.04, tmxSdsmPtr2->get_velocityCovariance().begin()->begin()->value, 0.0001); + EXPECT_EQ(0.0, tmxSdsmPtr2->get_velocityCovariance().begin()->back().value); + + EXPECT_NEAR(0.01, tmxSdsmPtr2->get_angularVelocityCovariance().begin()->begin()->value, 0.0001); + EXPECT_EQ(0.0, tmxSdsmPtr2->get_angularVelocityCovariance().begin()->back().value); + } +} \ No newline at end of file diff --git a/src/tmx/Messages/test/TestTimeSyncMessage.cpp b/src/tmx/Messages/test/TestTimeSyncMessage.cpp index a853ed2b2..13b27c69d 100644 --- a/src/tmx/Messages/test/TestTimeSyncMessage.cpp +++ b/src/tmx/Messages/test/TestTimeSyncMessage.cpp @@ -4,7 +4,7 @@ namespace tmx::messages { TEST(TestTimeSyncMessage, to_string) { TimeSyncMessage msg(20, 30); - std::string json = "{ \"timestep\":20, \"seq\":30}"; + std::string json = "{\"timestep\":\"20\",\"seq\":\"30\"}\n"; EXPECT_EQ( json, msg.to_string()); } } \ No newline at end of file diff --git a/src/tmx/TmxApi/tmx/messages/message.hpp b/src/tmx/TmxApi/tmx/messages/message.hpp index 3ac3960cf..94d26a88d 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 two_dimension_array_attribute(ELEMENT, NAME) \ + std::vector> get_##NAME () { return get_two_dimension_array(#NAME); } \ + void set_##NAME(std::vector> array) { return set_two_dimension_array(#NAME, array); } + #define object_attribute(ELEMENT, NAME) \ ELEMENT get_##NAME() {return get_object(#NAME); } \ void set_##NAME(ELEMENT obj) {return set_object(#NAME, obj); } \ @@ -407,6 +412,65 @@ class tmx_message { tree.get().push_back(typename message_tree_type::value_type("", Element::to_tree(element))); } + /** + * Get the entire contents of a two dimension array field as a vector. + * Note that the template Element type must contain methods with the following signatures: + * - static Element from_tree(message_tree_type&) + * - static message_tree_type to_tree(Element element) + * @param The name of the array field. + * @returns A two dimension array of all array elements. + */ + template + std::vector> get_two_dimension_array(std::string arrayName) + { + std::vector> ret; + boost::optional tree = this->as_tree(arrayName); + if(tree) + { + for(auto& outer_pair: tree.get()){ + std::vector temp; + for(auto& inner_pair: outer_pair.second){ + Element element = Element::from_tree(inner_pair.second); + temp.push_back(element); + } + ret.push_back(temp); + } + } + return ret; + } + + /** + * Set the entire contents of a two dimension array field. + * Note that the template Element type must contain methods with the following signatures: + * - static Element from_tree(message_tree_type&) + * - static message_tree_type to_tree(Element element) + * @param The name of the array field. + * @param array A two dimenstion array containing all elements to set. + */ + template + void set_two_dimension_array(std::string arrayName, std::vector> array) + { + erase_array(arrayName); + boost::optional tree = this->as_tree(arrayName); + if (!tree) + { + // Add the empty array + message_tree_type emptyTree; + this->as_tree().get().add_child(arrayName, emptyTree); + tree = this->as_tree(arrayName); + } + + for(auto& nested_array: array){ + boost::property_tree::ptree subtree; + //Populate nested array + for(auto& element: nested_array){ + subtree.push_back(typename message_tree_type::value_type("", Element::to_tree(element))); + } + //Add nested array + tree.get().push_back(typename message_tree_type::value_type("", subtree)); + } + } + /*** * @brief Get the content of an object fields * @param Name of the object diff --git a/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp b/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp deleted file mode 100644 index e2875396e..000000000 --- a/src/tmx/TmxUtils/test/SensorDetectedObjectTest.cpp +++ /dev/null @@ -1,79 +0,0 @@ -#include -#include "SensorDetectedObject.h" - -namespace tmx::messages{ - class SensorDetectedObjectTest : public testing::Test{ - protected: - std::shared_ptr tmxSdsmPtr; - SensorDetectedObjectTest(){ - tmxSdsmPtr = std::make_shared(); - } - 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 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(); - 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); - } -} \ No newline at end of file From 36e19e50a339af772465864dbfd14dc43462cbc5 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 12 Aug 2024 21:02:45 +0000 Subject: [PATCH 05/13] add messages pkg to sonar --- .sonarqube/sonar-scanner.properties | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index fdce595fc..f6c2ba1db 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -58,7 +58,8 @@ sonar.modules= PedestrianPlugin, \ CDASimAdapter, \ RSUHealthMonitorPlugin, \ TelematicBridgePlugin, \ - MUSTSensorDriverPlugin + MUSTSensorDriverPlugin, \ + Messages @@ -66,6 +67,7 @@ TmxCore.sonar.projectBaseDir =src/tmx/TmxCore TmxCtl.sonar.projectBaseDir =src/tmx/TmxCtl TmxTools.sonar.projectBaseDir =src/tmx/TmxTools TmxUtils.sonar.projectBaseDir =src/tmx/TmxUtils +Messages.sonar.projectBaseDir =src/tmx/Messages CARMACloudPlugin.sonar.projectBaseDir =src/v2i-hub/CARMACloudPlugin CommandPlugin.sonar.projectBaseDir =src/v2i-hub/CommandPlugin CswPlugin.sonar.projectBaseDir =src/v2i-hub/CswPlugin @@ -97,6 +99,7 @@ TmxCore.sonar.sources =src TmxCtl.sonar.sources =src TmxTools.sonar.sources =src TmxUtils.sonar.sources =src +Messages.sonar.sources =src TmxUtils.sonar.exclusions =test/** MessageLoggerPlugin.sonar.sources =src CswPlugin.sonar.sources =src @@ -132,6 +135,7 @@ MUSTSensorDriverPlugin.sonar.sources =src # Tests # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated. TmxUtils.sonar.tests=test +Messages.sonar.tests=test #TmxCore.sonar.tests=test #TmxCtl.sonar.tests=test #TmxTools.sonar.tests=test From 3e7c0d3e3ead1aacf3cc68ef528f1d56995cf0dd Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 12 Aug 2024 21:37:08 +0000 Subject: [PATCH 06/13] fix must sensor --- .../src/MUSTSensorDetection.cpp | 16 +++++++-------- .../test/TestMUSTSensorDetection.cpp | 20 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp index b193f82b8..8d37af661 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.set_ObjectId(detection.trackID); + 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(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)); + 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 { diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp index 0d8fb0e22..0f745df77 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.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()); + 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 ) { From 4d4d59d208829e71917d22d8b1ac6fea81a024db Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 12 Aug 2024 21:44:57 +0000 Subject: [PATCH 07/13] fix no file SensorDetectedObject.h --- src/tmx/Messages/include/SensorDetectedObject.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h index 504eb910d..802401338 100644 --- a/src/tmx/Messages/include/SensorDetectedObject.h +++ b/src/tmx/Messages/include/SensorDetectedObject.h @@ -1,5 +1,4 @@ -#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_ -#define INCLUDE_SIMULATED_SensorDetectedObject_H_ +#pragma once #include #include @@ -67,4 +66,3 @@ namespace tmx } }; // namespace tmx -#endif From 63b51d509093f2307f41623f1555367ab1ff656a Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 12 Aug 2024 22:34:11 +0000 Subject: [PATCH 08/13] update --- src/tmx/Messages/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tmx/Messages/CMakeLists.txt b/src/tmx/Messages/CMakeLists.txt index 89d08688d..634a5a142 100644 --- a/src/tmx/Messages/CMakeLists.txt +++ b/src/tmx/Messages/CMakeLists.txt @@ -18,5 +18,5 @@ file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) add_executable(${BINARY} ${TEST_SOURCES}) add_test(NAME ${BINARY} COMMAND ${BINARY}) - +target_include_directories(${BINARY} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) target_link_libraries(${BINARY} PUBLIC ${TMXAPI_LIBRARIES} gtest) \ No newline at end of file From adfd407a06779bfc4b070bc0e48fbf1273659ecf Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 12 Aug 2024 23:06:16 +0000 Subject: [PATCH 09/13] remove unused import --- src/tmx/Messages/include/SensorDetectedObject.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h index 802401338..2a430afb6 100644 --- a/src/tmx/Messages/include/SensorDetectedObject.h +++ b/src/tmx/Messages/include/SensorDetectedObject.h @@ -2,8 +2,6 @@ #include #include -#include -#include #include "Position.h" #include "Covariance.h" #include "Velocity.h" From 0dde9cee4b7e18e301a2eb2eb2889818539beb83 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 13 Aug 2024 00:50:25 +0000 Subject: [PATCH 10/13] update sonar source --- .sonarqube/sonar-scanner.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index f6c2ba1db..22232d9f9 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -99,7 +99,7 @@ TmxCore.sonar.sources =src TmxCtl.sonar.sources =src TmxTools.sonar.sources =src TmxUtils.sonar.sources =src -Messages.sonar.sources =src +Messages.sonar.sources =include TmxUtils.sonar.exclusions =test/** MessageLoggerPlugin.sonar.sources =src CswPlugin.sonar.sources =src From 06dd95731ad9bd5574b1fcf1c003dda0c849aba9 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 13 Aug 2024 02:13:07 +0000 Subject: [PATCH 11/13] address code smell --- src/tmx/Messages/include/Covariance.h | 35 +++--- src/tmx/Messages/include/Position.h | 45 ++++---- .../Messages/include/SensorDetectedObject.h | 105 +++++++++--------- src/tmx/Messages/include/Size.h | 49 ++++---- src/tmx/Messages/include/Velocity.h | 8 +- .../test/SensorDetectedObjectTest.cpp | 3 + 6 files changed, 118 insertions(+), 127 deletions(-) diff --git a/src/tmx/Messages/include/Covariance.h b/src/tmx/Messages/include/Covariance.h index 1259824a3..622eecf52 100644 --- a/src/tmx/Messages/include/Covariance.h +++ b/src/tmx/Messages/include/Covariance.h @@ -1,23 +1,20 @@ #pragma once #include -namespace tmx +namespace tmx::messages { - namespace messages - { - typedef struct Covariance{ - double value; - Covariance(){}; - Covariance(double 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; - } + typedef struct Covariance{ + double value; + Covariance()=default; + explicit Covariance(double 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; } \ No newline at end of file diff --git a/src/tmx/Messages/include/Position.h b/src/tmx/Messages/include/Position.h index 22f8e8501..4dbaa651f 100644 --- a/src/tmx/Messages/include/Position.h +++ b/src/tmx/Messages/include/Position.h @@ -1,28 +1,25 @@ #pragma once #include -namespace tmx +namespace tmx::messages { - 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("x"); - pos.y = tree.get("y"); - pos.z = tree.get("z"); - return pos; - } - } Position; - } + // Cartesian positiion of object. Assumed to be ENU coordinate frame. + typedef struct Position{ + double x, y, z; + Position()=default; + explicit 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; } \ No newline at end of file diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h index 2a430afb6..4293e4ed8 100644 --- a/src/tmx/Messages/include/SensorDetectedObject.h +++ b/src/tmx/Messages/include/SensorDetectedObject.h @@ -7,60 +7,55 @@ #include "Velocity.h" #include "Size.h" -namespace tmx +namespace tmx::messages { - namespace messages - { - /** - * This SensorDetectedObject is used to communicate the sensor detected object information with various applications. - * This message is the generic representation of a sensor detection. - */ - class SensorDetectedObject : public tmx::message - { - public: - SensorDetectedObject(){}; - SensorDetectedObject(const tmx::message_container_type &contents) : tmx::message(contents) {}; - ~SensorDetectedObject(){}; - // Message type for routing this message through TMX core - static constexpr const char *MessageType = MSGTYPE_APPLICATION_STRING; - - // // Message sub type for routing this message through TMX core - 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 ) - //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 - 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); - two_dimension_array_attribute(Covariance, positionCovariance); - //Linear velocity in meter per second - object_attribute(Velocity, velocity); - //Covariance associated with linear velocity. - two_dimension_array_attribute(Covariance, velocityCovariance); - //Angular velocity in radians per second. - object_attribute(Velocity, angularVelocity); - //Covariance associated with angular velocity. - two_dimension_array_attribute(Covariance, angularVelocityCovariance); - - // Epoch time in milliseconds. - // long timestamp = 0; - std_attribute(this->msg, long, timestamp, 0, ); - object_attribute(Size, size); - - }; - - } - -}; // namespace tmx + /** + * This SensorDetectedObject is used to communicate the sensor detected object information with various applications. + * This message is the generic representation of a sensor detection. + */ + class SensorDetectedObject : public tmx::message + { + public: + SensorDetectedObject(){}; + SensorDetectedObject(const tmx::message_container_type &contents) : tmx::message(contents) {}; + ~SensorDetectedObject() override{}; + // Message type for routing this message through TMX core + static constexpr const char *MessageType = MSGTYPE_APPLICATION_STRING; + + // // Message sub type for routing this message through TMX core + static constexpr const char *MessageSubType = MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING; + + //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 + 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); + two_dimension_array_attribute(Covariance, positionCovariance); + //Linear velocity in meter per second + object_attribute(Velocity, velocity); + //Covariance associated with linear velocity. + two_dimension_array_attribute(Covariance, velocityCovariance); + //Angular velocity in radians per second. + object_attribute(Velocity, angularVelocity); + //Covariance associated with angular velocity. + two_dimension_array_attribute(Covariance, angularVelocityCovariance); + + // Epoch time in milliseconds. + // long timestamp = 0; + std_attribute(this->msg, long, timestamp, 0, ); + object_attribute(Size, size); + + }; + +} \ No newline at end of file diff --git a/src/tmx/Messages/include/Size.h b/src/tmx/Messages/include/Size.h index 7953ccbe8..8f2bb7768 100644 --- a/src/tmx/Messages/include/Size.h +++ b/src/tmx/Messages/include/Size.h @@ -1,30 +1,27 @@ #pragma once #include -namespace tmx +namespace tmx::messages { - 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("length"); - size.width = tree.get("width"); - size.height = tree.get("height"); - return size; - } - } Size; - } + //Length, width and height of object in meter. + typedef struct Size{ + double length; + double width; + double height; + Size()=default; + explicit 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; } \ No newline at end of file diff --git a/src/tmx/Messages/include/Velocity.h b/src/tmx/Messages/include/Velocity.h index 28feb4b25..df931c36d 100644 --- a/src/tmx/Messages/include/Velocity.h +++ b/src/tmx/Messages/include/Velocity.h @@ -6,9 +6,11 @@ namespace tmx { // 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){}; + double x; + double y; + double z; + Velocity()=default; + explicit 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); diff --git a/src/tmx/Messages/test/SensorDetectedObjectTest.cpp b/src/tmx/Messages/test/SensorDetectedObjectTest.cpp index 279cbcf26..22bcbaf72 100644 --- a/src/tmx/Messages/test/SensorDetectedObjectTest.cpp +++ b/src/tmx/Messages/test/SensorDetectedObjectTest.cpp @@ -93,8 +93,11 @@ namespace tmx::messages{ EXPECT_EQ(110200, tmxSdsmPtr2->get_timestamp()); EXPECT_EQ(3, tmxSdsmPtr2->get_positionCovariance().size()); + EXPECT_EQ(3, tmxSdsmPtr2->get_positionCovariance().begin()->size()); EXPECT_EQ(3, tmxSdsmPtr2->get_angularVelocityCovariance().size()); + EXPECT_EQ(3, tmxSdsmPtr2->get_angularVelocityCovariance().begin()->size()); EXPECT_EQ(3, tmxSdsmPtr2->get_velocityCovariance().size()); + EXPECT_EQ(3, tmxSdsmPtr2->get_velocityCovariance().begin()->size()); EXPECT_NEAR(0.04,tmxSdsmPtr2->get_positionCovariance().begin()->begin()->value, 0.0001); EXPECT_EQ(0.0, tmxSdsmPtr2->get_positionCovariance().begin()->back().value); From 2f7e112092864a24d935210e0e8c2d0e1f2225e4 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 13 Aug 2024 02:52:01 +0000 Subject: [PATCH 12/13] address code smell --- src/tmx/Messages/include/Position.h | 4 +- .../Messages/include/SensorDetectedObject.h | 4 +- src/tmx/Messages/include/Velocity.h | 49 +++++++++---------- 3 files changed, 28 insertions(+), 29 deletions(-) diff --git a/src/tmx/Messages/include/Position.h b/src/tmx/Messages/include/Position.h index 4dbaa651f..96fa2e2c8 100644 --- a/src/tmx/Messages/include/Position.h +++ b/src/tmx/Messages/include/Position.h @@ -4,7 +4,9 @@ namespace tmx::messages { // Cartesian positiion of object. Assumed to be ENU coordinate frame. typedef struct Position{ - double x, y, z; + double x; + double y; + double z; Position()=default; explicit Position(double x, double y, double z):x(x),y(y),z(z){}; static message_tree_type to_tree(const Position& pos){ diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h index 4293e4ed8..4def40746 100644 --- a/src/tmx/Messages/include/SensorDetectedObject.h +++ b/src/tmx/Messages/include/SensorDetectedObject.h @@ -17,8 +17,8 @@ namespace tmx::messages class SensorDetectedObject : public tmx::message { public: - SensorDetectedObject(){}; - SensorDetectedObject(const tmx::message_container_type &contents) : tmx::message(contents) {}; + SensorDetectedObject()=default; + explicit SensorDetectedObject(const tmx::message_container_type &contents) : tmx::message(contents) {}; ~SensorDetectedObject() override{}; // Message type for routing this message through TMX core static constexpr const char *MessageType = MSGTYPE_APPLICATION_STRING; diff --git a/src/tmx/Messages/include/Velocity.h b/src/tmx/Messages/include/Velocity.h index df931c36d..8827f53a3 100644 --- a/src/tmx/Messages/include/Velocity.h +++ b/src/tmx/Messages/include/Velocity.h @@ -1,30 +1,27 @@ #pragma once #include -namespace tmx +namespace tmx::messages { - namespace messages - { - // Cartesian velocity vector of object. Assumed to be ENU coordinate frame. - typedef struct Velocity{ - double x; - double y; - double z; - Velocity()=default; - explicit 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; - } + // Cartesian velocity vector of object. Assumed to be ENU coordinate frame. + typedef struct Velocity{ + double x; + double y; + double z; + Velocity()=default; + explicit 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; } \ No newline at end of file From a573e127de597374fc2093e6f4dec3130c001ca5 Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Tue, 13 Aug 2024 15:07:28 +0000 Subject: [PATCH 13/13] address comments --- src/tmx/Messages/include/Covariance.h | 4 +- src/tmx/Messages/include/Position.h | 4 +- src/tmx/Messages/include/Size.h | 4 +- src/tmx/Messages/include/Velocity.h | 4 +- .../test/SensorDetectedObjectTest.cpp | 83 ++++++++++++++++++- 5 files changed, 90 insertions(+), 9 deletions(-) diff --git a/src/tmx/Messages/include/Covariance.h b/src/tmx/Messages/include/Covariance.h index 622eecf52..ebf7dca53 100644 --- a/src/tmx/Messages/include/Covariance.h +++ b/src/tmx/Messages/include/Covariance.h @@ -2,7 +2,7 @@ #include namespace tmx::messages { - typedef struct Covariance{ + struct Covariance{ double value; Covariance()=default; explicit Covariance(double value):value(value){}; @@ -16,5 +16,5 @@ namespace tmx::messages cov.value = tree.get(""); return cov; } - } Covariance; + }; } \ No newline at end of file diff --git a/src/tmx/Messages/include/Position.h b/src/tmx/Messages/include/Position.h index 96fa2e2c8..8e0d2b029 100644 --- a/src/tmx/Messages/include/Position.h +++ b/src/tmx/Messages/include/Position.h @@ -3,7 +3,7 @@ namespace tmx::messages { // Cartesian positiion of object. Assumed to be ENU coordinate frame. - typedef struct Position{ + struct Position{ double x; double y; double z; @@ -23,5 +23,5 @@ namespace tmx::messages pos.z = tree.get("z"); return pos; } - } Position; + }; } \ No newline at end of file diff --git a/src/tmx/Messages/include/Size.h b/src/tmx/Messages/include/Size.h index 8f2bb7768..c94e72362 100644 --- a/src/tmx/Messages/include/Size.h +++ b/src/tmx/Messages/include/Size.h @@ -3,7 +3,7 @@ namespace tmx::messages { //Length, width and height of object in meter. - typedef struct Size{ + struct Size{ double length; double width; double height; @@ -23,5 +23,5 @@ namespace tmx::messages size.height = tree.get("height"); return size; } - } Size; + }; } \ No newline at end of file diff --git a/src/tmx/Messages/include/Velocity.h b/src/tmx/Messages/include/Velocity.h index 8827f53a3..e8eed0e92 100644 --- a/src/tmx/Messages/include/Velocity.h +++ b/src/tmx/Messages/include/Velocity.h @@ -3,7 +3,7 @@ namespace tmx::messages { // Cartesian velocity vector of object. Assumed to be ENU coordinate frame. - typedef struct Velocity{ + struct Velocity{ double x; double y; double z; @@ -23,5 +23,5 @@ namespace tmx::messages velocity.z = tree.get("z"); return velocity; } - } Velocity; + }; } \ No newline at end of file diff --git a/src/tmx/Messages/test/SensorDetectedObjectTest.cpp b/src/tmx/Messages/test/SensorDetectedObjectTest.cpp index 22bcbaf72..7a49e3ce0 100644 --- a/src/tmx/Messages/test/SensorDetectedObjectTest.cpp +++ b/src/tmx/Messages/test/SensorDetectedObjectTest.cpp @@ -69,7 +69,88 @@ namespace tmx::messages{ TEST_F(SensorDetectedObjectTest, deserialize){ auto tmxSdsmPtr2 = std::make_shared(); - std::string expectedStr = "{\"isSimulated\":1,\"type\":\"CAR\",\"confidence\":1.0,\"sensorId\":\"IntersectionLidar\",\"projString\":\"+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\",\"objectId\":207,\"position\":{\"x\":-5.021,\"y\":64.234,\"z\":-10.297},\"positionCovariance\":[[0.04000000000000001,0.0,0.0],[0.0,0.04000000000000001,0.0],[0.0,0.0,0.04000000000000001]],\"velocity\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"velocityCovariance\":[[0.04000000000000001,0.0,0.0],[0.0,0.04000000000000001,0.0],[0.0,0.0,0.04000000000000001]],\"angularVelocity\":{\"x\":0.0,\"y\":-0.0,\"z\":-0.0},\"angularVelocityCovariance\":[[0.010000000000000002,0.0,0.0],[0.0,0.010000000000000002,0.0],[0.0,0.0,0.010000000000000002]],\"size\":{\"length\":2.257,\"height\":1.003,\"width\":0.762},\"timestamp\":110200}"; + std::string expectedStr = R"( + { + "isSimulated": 1, + "type": "CAR", + "confidence": 1, + "sensorId": "IntersectionLidar", + "projString": "+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", + "objectId": 207, + "position": { + "x": -5.021, + "y": 64.234, + "z": -10.297 + }, + "positionCovariance": [ + [ + 0.04000000000000001, + 0, + 0 + ], + [ + 0, + 0.04000000000000001, + 0 + ], + [ + 0, + 0, + 0.04000000000000001 + ] + ], + "velocity": { + "x": 0, + "y": 0, + "z": 0 + }, + "velocityCovariance": [ + [ + 0.04000000000000001, + 0, + 0 + ], + [ + 0, + 0.04000000000000001, + 0 + ], + [ + 0, + 0, + 0.04000000000000001 + ] + ], + "angularVelocity": { + "x": 0, + "y": 0, + "z": 0 + }, + "angularVelocityCovariance": [ + [ + 0.010000000000000002, + 0, + 0 + ], + [ + 0, + 0.010000000000000002, + 0 + ], + [ + 0, + 0, + 0.010000000000000002 + ] + ], + "size": { + "length": 2.257, + "height": 1.003, + "width": 0.762 + }, + "timestamp": 110200 + } + )"; tmxSdsmPtr2->set_contents(expectedStr); EXPECT_EQ(expectedStr, tmxSdsmPtr2->to_string()); EXPECT_EQ(true, tmxSdsmPtr2->get_isSimulated());