From d2bbdd11cee5c4ff1070237aeb9428775c21d187 Mon Sep 17 00:00:00 2001 From: dev Date: Tue, 18 Jun 2024 19:29:17 -0400 Subject: [PATCH] Updates --- .../Messages/include/SensorDetectedObject.h | 44 +++++++++++++++++++ .../include/simulation/SensorDetectedObject.h | 44 ------------------- src/tmx/TmxUtils/src/Vector3d.h | 19 ++++++++ .../src/CARMAStreetsPlugin.cpp | 4 +- .../src/CARMAStreetsPlugin.h | 4 +- .../CDASimAdapter/src/CDASimAdapter.cpp | 4 +- .../CDASimAdapter/src/CDASimConnection.cpp | 4 +- .../src/include/CDASimAdapter.hpp | 2 +- .../src/include/CDASimConnection.hpp | 4 +- .../src/MUSTSensorDetection.cpp | 13 +++++- .../src/MUSTSensorDetection.h | 6 ++- .../src/MUSTSensorDriverPlugin.cpp | 4 +- .../src/MUSTSensorDriverPlugin.h | 2 +- .../test/TestMUSTSensorDetection.cpp | 35 +++++++++++++++ 14 files changed, 127 insertions(+), 62 deletions(-) create mode 100644 src/tmx/Messages/include/SensorDetectedObject.h delete mode 100644 src/tmx/Messages/include/simulation/SensorDetectedObject.h create mode 100644 src/tmx/TmxUtils/src/Vector3d.h diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h new file mode 100644 index 000000000..4df3b3ca6 --- /dev/null +++ b/src/tmx/Messages/include/SensorDetectedObject.h @@ -0,0 +1,44 @@ +#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_ +#define INCLUDE_SIMULATED_SensorDetectedObject_H_ + +#include +#include +#include +#include + +namespace tmx +{ + namespace messages + { + + /** + * This SensorDetectedObject is used to communicate the sensor detected object information with various applications + * including internal infrastructure applications and external road user applications through simulated environment. + * It defines the message type and sub type and all data members. + */ + 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; + + std::string type = ""; + double confidence = 0.0; + std::string sensorId = ""; + std::string projString = ""; + int objectId = 0; + tmx::utils::Point position = tmx::utils::Point(); + tmx::utils::Vector3d velocity = tmx::utils::Vector3d(); + long timestamp = 0; + }; + + } + +}; // namespace tmx +#endif diff --git a/src/tmx/Messages/include/simulation/SensorDetectedObject.h b/src/tmx/Messages/include/simulation/SensorDetectedObject.h deleted file mode 100644 index 5c7c33d4c..000000000 --- a/src/tmx/Messages/include/simulation/SensorDetectedObject.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_ -#define INCLUDE_SIMULATED_SensorDetectedObject_H_ - -#include -#include -#include - -namespace tmx -{ - namespace messages - { - namespace simulation - { - /** - * This SensorDetectedObject is used to communicate the sensor detected object information with various applications - * including internal infrastructure applications and external road user applications through simulated environment. - * It defines the message type and sub type and all data members. - */ - 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; - - std::string type; - double confidence; - std::string sensorId; - std::string projString; - int objectId; - tmx::utils::Point position; - tmx::utils::Point velocity; - long timestamp; - }; - } - } - -}; // namespace tmx -#endif diff --git a/src/tmx/TmxUtils/src/Vector3d.h b/src/tmx/TmxUtils/src/Vector3d.h new file mode 100644 index 000000000..d8e6a1dfb --- /dev/null +++ b/src/tmx/TmxUtils/src/Vector3d.h @@ -0,0 +1,19 @@ +#pragma once + +namespace tmx::utils { + + + /// Three dimensional Vector + typedef struct Vector3d + { + Vector3d() : X(0), Y(0), Z(0) {} + + Vector3d(double x, double y, double z = 0.0): + X(x), Y(y), Z(z) { } + + double X; + double Y; + double Z; + } Vector3d; + +} // namespace tmx::utils diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index d0d4d27be..9a23f0324 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,7 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); - AddMessageFilter < simulation::SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage ); + AddMessageFilter < SensorDetectedObject > (this, &CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage ); SubscribeToMessages(); } @@ -704,7 +704,7 @@ void CARMAStreetsPlugin::SubscribeSDSMKafkaTopic(){ } -void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) +void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(SensorDetectedObject &msg, routeable_message &routeableMsg) { // TODO: This is a temporary fix for tmx message container property tree // serializing all attributes as strings. This issue needs to be fixed but diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h index f6008d76a..74ec40cb1 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h @@ -20,7 +20,7 @@ #include #include #include "JsonToJ2735SSMConverter.h" -#include +#include #include "JsonToJ3224SDSMConverter.h" #include "J3224ToSDSMJsonConverter.h" #include "PluginClientClockAware.h" @@ -56,7 +56,7 @@ class CARMAStreetsPlugin: public PluginClientClockAware { * @param msg Detected object received from TMX bus. * @param routeableMsg routeable_message for detected object. */ - void HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg); + void HandleSimulatedSensorDetectedMessage(SensorDetectedObject &msg, routeable_message &routeableMsg); /** * @brief Overide PluginClientClockAware HandleTimeSyncMessage to producer TimeSyncMessage to kafka for CARMA Streets Time Synchronization. * @param msg TimeSyncMessage received by plugin when in simulation mode. Message provides current simulation time to all processes. diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index 38eb994ab..0a535e063 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -87,9 +87,9 @@ namespace CDASimAdapter{ } - void CDASimAdapter::forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg) { + void CDASimAdapter::forward_simulated_detected_message(tmx::messages::SensorDetectedObject &msg) { PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl; - this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); + this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); } bool CDASimAdapter::connect() { diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 07b26e516..4e6d08ab4 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -142,9 +142,9 @@ namespace CDASimAdapter{ } - tmx::messages::simulation::SensorDetectedObject CDASimConnection::consume_sensor_detected_object_message() const + tmx::messages::SensorDetectedObject CDASimConnection::consume_sensor_detected_object_message() const { - tmx::messages::simulation::SensorDetectedObject externalObj; + tmx::messages::SensorDetectedObject externalObj; externalObj.clear(); if(sensor_detected_object_listener) { diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp index 529cd9dc9..569be2eec 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -93,7 +93,7 @@ namespace CDASimAdapter * @brief Forward simulated sensor detected object message to TMX message bus for other V2X-Hub Plugin * @param msg simulation::SensorDetectedObject. */ - void forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg); + void forward_simulated_detected_message(tmx::messages::SensorDetectedObject &msg); /** * @brief Method to start thread timer for regular interval actions lauched on seperate thread. */ diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index 2c8a03bf5..b79cd3534 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include #include @@ -88,7 +88,7 @@ namespace CDASimAdapter { * To populate the simulation external object, this JSON string has to follow this specification: https://usdot-carma.atlassian.net/wiki/spaces/CRMSIM/pages/2563899417/Detected+Objects+Specification#CARMA-Street-and-V2xHub * @return simulation::SensorDetectedObject. */ - tmx::messages::simulation::SensorDetectedObject consume_sensor_detected_object_message() const; + tmx::messages::SensorDetectedObject consume_sensor_detected_object_message() const; /** * @brief Perform handshake with CARMA-Simulation. Will return true on successful handshakes and false if * unsuccessful. As part of the handshake should set simulation_v2x_port for forwarding v2x messages to simulation, diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp index 8100462c1..eb641bae0 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp @@ -29,13 +29,14 @@ namespace MUSTSensorDriverPlugin { return detection; } - tmx::messages::simulation::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection) { - tmx::messages::simulation::SensorDetectedObject detectedObject; + tmx::messages::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection) { + 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 = detection.timestamp; + detectedObject.velocity = headingSpeedToVelocity(detection.heading, detection.speed); return detectedObject; } const DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept { @@ -57,4 +58,12 @@ namespace MUSTSensorDriverPlugin { return DetectionSize::NA; } }; + + tmx::utils::Vector3d headingSpeedToVelocity(double heading, double speed) { + tmx::utils::Vector3d velocity; + velocity.X = - std::sin(M_PI/180* heading) * speed; + velocity.Y = std::cos(M_PI/180 * heading) * speed; + velocity.Z = 0; + return velocity; + }; } \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h index 3f474e30b..8f0d089b6 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h @@ -2,8 +2,9 @@ #include #include #include // std::out_of_range -#include +#include #include +#include namespace MUSTSensorDriverPlugin { @@ -54,6 +55,7 @@ namespace MUSTSensorDriverPlugin { MUSTSensorDetection csvToDectection(const std::string &csv ); - tmx::messages::simulation::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection); + tmx::messages::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection); + tmx::utils::Vector3d headingSpeedToVelocity(double heading, double speed); } \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp index d3f2c8991..6a5fe15eb 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp @@ -51,9 +51,9 @@ namespace MUSTSensorDriverPlugin { void MUSTSensorDriverPlugin::processMUSTSensorDetection(){ if (mustSensorPacketReceiver) { MUSTSensorDetection detection = csvToDectection(mustSensorPacketReceiver->stringTimedReceive()); - tmx::messages::simulation::SensorDetectedObject msg = mustDectionToSensorDetectedObject(detection); + tmx::messages::SensorDetectedObject msg = mustDectionToSensorDetectedObject(detection); PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl; - this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); + this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); } } diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h index 3ac68c1c8..5caca06ef 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include "MUSTSensorDetection.h" diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp index d6b55ec40..6cd9fdb45 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp @@ -1,5 +1,6 @@ #include #include +#include using namespace MUSTSensorDriverPlugin; @@ -33,4 +34,38 @@ TEST(TestMUSTSensorDetection, csvToDectection ){ EXPECT_DOUBLE_EQ(detection.confidence, 95.1); EXPECT_EQ(detection.trackID, 1); EXPECT_EQ(detection.timestamp, 1714374738); +} + +TEST(TestMUSTSensorDetection, csvToDectectionInvalidCSV ){ + std::string valid_csv_data = "truck,13.3,22.4,30.5,35.8,large,95.1,1,1714374738,20"; + EXPECT_THROW(csvToDectection(valid_csv_data), std::runtime_error); +} + +TEST(TestMUSTSensorDetection, csvToDectectionEmptyString ){ + std::string valid_csv_data = ""; + EXPECT_THROW(csvToDectection(valid_csv_data), std::runtime_error); +} + +TEST(TestMUSTSensorDetection, mustDectionToSensorDetectedObject ) { + using namespace std::chrono; + + MUSTSensorDetection detection; + detection.cl = DetectionClassification::SEDAN; + detection.confidence = 95.5; + detection.heading = 330; + detection.position_x = 10.5; + detection.position_y = -20.3; + detection.size = DetectionSize::SMALL; + detection.timestamp = duration_cast(system_clock::now().time_since_epoch()).count(); + detection.trackID = 324; + detection.speed = 5; + + auto sensorDetectedObject = mustDectionToSensorDetectedObject(detection); + + 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); } \ No newline at end of file