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