From be5a5af9f314b23d2c61ea359989e2d27f3ee3d9 Mon Sep 17 00:00:00 2001 From: dan-du-car <62157949+dan-du-car@users.noreply.github.com> Date: Tue, 13 Aug 2024 12:40:38 -0400 Subject: [PATCH] FCP-5: SensorDetectedObject TMX message definition (#628) # PR Details ## Description Updating SensorDetectedObject message to allow for TMX JSON serialization/deserialization. ## Related Issue https://usdot-carma.atlassian.net/browse/FCP-5 ## Motivation and Context Freight Cooperative perception ## How Has This Been Tested? Unit test ## Types of changes - [x] Defect fix (non-breaking change that fixes an issue) - [ ] New feature (non-breaking change that adds functionality) - [ ] Breaking change (fix or feature that cause existing functionality to change) ## Checklist: - [ ] I have added any new packages to the sonar-scanner.properties file - [ ] My change requires a change to the documentation. - [ ] I have updated the documentation accordingly. - [x] I have read the **CONTRIBUTING** document. [V2XHUB Contributing Guide](https://github.com/usdot-fhwa-OPS/V2X-Hub/blob/develop/Contributing.md) - [ ] I have added tests to cover my changes. - [ ] All new and existing tests passed. --- .sonarqube/sonar-scanner.properties | 6 +- src/tmx/Messages/CMakeLists.txt | 15 ++ src/tmx/Messages/include/Covariance.h | 20 ++ src/tmx/Messages/include/Position.h | 27 +++ .../Messages/include/SensorDetectedObject.h | 95 +++++---- src/tmx/Messages/include/Size.h | 27 +++ src/tmx/Messages/include/Velocity.h | 27 +++ .../test/SensorDetectedObjectTest.cpp | 192 ++++++++++++++++++ src/tmx/Messages/test/TestTimeSyncMessage.cpp | 2 +- src/tmx/TmxApi/tmx/messages/message.hpp | 101 +++++++++ .../src/MUSTSensorDetection.cpp | 28 +-- .../src/MUSTSensorDetection.h | 2 +- .../test/TestMUSTSensorDetection.cpp | 24 +-- 13 files changed, 493 insertions(+), 73 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 create mode 100644 src/tmx/Messages/test/SensorDetectedObjectTest.cpp diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index fdce595fc..22232d9f9 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 =include 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 diff --git a/src/tmx/Messages/CMakeLists.txt b/src/tmx/Messages/CMakeLists.txt index 3b866ee13..634a5a142 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_include_directories(${BINARY} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) +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 new file mode 100644 index 000000000..ebf7dca53 --- /dev/null +++ b/src/tmx/Messages/include/Covariance.h @@ -0,0 +1,20 @@ +#pragma once +#include +namespace tmx::messages +{ + 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; + } + }; +} \ 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..8e0d2b029 --- /dev/null +++ b/src/tmx/Messages/include/Position.h @@ -0,0 +1,27 @@ +#pragma once +#include +namespace tmx::messages +{ + // Cartesian positiion of object. Assumed to be ENU coordinate frame. + struct Position{ + 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){ + 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; + } + }; +} \ No newline at end of file diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h index d275bbf7e..4def40746 100644 --- a/src/tmx/Messages/include/SensorDetectedObject.h +++ b/src/tmx/Messages/include/SensorDetectedObject.h @@ -1,54 +1,61 @@ -#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_ -#define INCLUDE_SIMULATED_SensorDetectedObject_H_ +#pragma once #include #include -#include -#include +#include "Position.h" +#include "Covariance.h" +#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; + /** + * 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()=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; + + // // Message sub type for routing this message through TMX core + static constexpr const char *MessageSubType = MSGSUBTYPE_SENSOR_DETECTED_OBJECT_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, ); - // 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 = ""; - // Confidence of type classification - double confidence = 0.0; - // Unique indentifier of sensor reporting detection - std::string sensorId = ""; - // String describing projection used to convert cartesian data to WGS84 data - std::string projString = ""; - // Unique identifier of detected object - int objectId = 0; - // Cartesian positiion of object. Assumed to be ENU coordinate frame. - tmx::utils::Point position = tmx::utils::Point(); - // Cartesian velocity vector of object. Assumed to be ENU coordinate frame. - tmx::utils::Vector3d velocity = tmx::utils::Vector3d(); - // Epoch time in milliseconds - long timestamp = 0; - - }; + + 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 -#endif +} \ No newline at end of file diff --git a/src/tmx/Messages/include/Size.h b/src/tmx/Messages/include/Size.h new file mode 100644 index 000000000..c94e72362 --- /dev/null +++ b/src/tmx/Messages/include/Size.h @@ -0,0 +1,27 @@ +#pragma once +#include +namespace tmx::messages +{ + //Length, width and height of object in meter. + 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; + } + }; +} \ 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..e8eed0e92 --- /dev/null +++ b/src/tmx/Messages/include/Velocity.h @@ -0,0 +1,27 @@ +#pragma once +#include +namespace tmx::messages +{ + // Cartesian velocity vector of object. Assumed to be ENU coordinate frame. + 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; + } + }; +} \ No newline at end of file diff --git a/src/tmx/Messages/test/SensorDetectedObjectTest.cpp b/src/tmx/Messages/test/SensorDetectedObjectTest.cpp new file mode 100644 index 000000000..7a49e3ce0 --- /dev/null +++ b/src/tmx/Messages/test/SensorDetectedObjectTest.cpp @@ -0,0 +1,192 @@ +#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 = 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()); + 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_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); + + 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 18570d558..94d26a88d 100644 --- a/src/tmx/TmxApi/tmx/messages/message.hpp +++ b/src/tmx/TmxApi/tmx/messages/message.hpp @@ -22,6 +22,16 @@ 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); } \ + void erase_##NAME(){erase_object(#NAME); } + namespace tmx { @@ -402,6 +412,97 @@ 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 + * @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)); + } + /** + * @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); + } + /** * Erase the entire contents of an array field. * @param The name of the array field. diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp index 95b514d9e..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.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::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::Velocity headingSpeedToVelocity(double heading, double speed) { // Convert North East heading to Angle with 0 at (1, 0) (See README Unit Circle) heading = heading - 270; // factor for converting heading from degrees to radians auto headingInRad = M_PI / 180; - tmx::utils::Vector3d velocity; - velocity.X = std::cos(headingInRad * heading) * speed; - velocity.Y = std::sin(headingInRad * heading) * speed; - velocity.Z = 0; + tmx::messages::Velocity velocity; + velocity.x = std::cos(headingInRad * heading) * speed; + velocity.y = std::sin(headingInRad * heading) * speed; + velocity.z = 0; return velocity; }; } diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h index 313204811..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::utils::Vector3d headingSpeedToVelocity(double heading, double speed); + tmx::messages::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..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.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