Updates

src/tmx/Messages/include/SensorDetectedObject.h

@@ -0,0 +1,44 @@
+#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_
+#define INCLUDE_SIMULATED_SensorDetectedObject_H_
+
+#include <tmx/messages/message.hpp>
+#include <MessageTypes.h>
+#include <Vector3d.h>
+#include <Point.h>
+
+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

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

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

src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.h

@@ -20,7 +20,7 @@
 #include <kafka/kafka_client.h>
 #include <kafka/kafka_consumer_worker.h>
 #include "JsonToJ2735SSMConverter.h"
-#include <simulation/SensorDetectedObject.h>
+#include <SensorDetectedObject.h>
 #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.

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<tmx::messages::simulation::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
+        this->BroadcastMessage<tmx::messages::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
     }
 
     bool CDASimAdapter::connect() {

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)
         {

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.
          */

src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp

@@ -4,7 +4,7 @@
 #include <tmx/tmx.h>
 #include <Point.h>
 #include <TimeSyncMessage.h>
-#include <simulation/SensorDetectedObject.h>
+#include <SensorDetectedObject.h>
 #include <jsoncpp/json/json.h>
 #include <PluginLog.h>
 #include <gtest/gtest.h>
@@ -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,

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;
+    };
 }

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h

@@ -2,8 +2,9 @@
 #include <string>
 #include <unordered_map>
 #include <stdexcept>      // std::out_of_range
-#include <simulation/SensorDetectedObject.h>
+#include <SensorDetectedObject.h>
 #include <PluginLog.h>
+#include <Vector3d.h>
 
 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);
 }

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<tmx::messages::simulation::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
+       		this->BroadcastMessage<tmx::messages::SensorDetectedObject>(msg, _name, 0 , IvpMsgFlags_None);
 		}
 	}
 

src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h

@@ -19,7 +19,7 @@
 #include <UdpServer.h>
 #include <ThreadWorker.h>
 #include <mutex>
-#include <simulation/SensorDetectedObject.h>
+#include <SensorDetectedObject.h>
 
 #include "MUSTSensorDetection.h"
 

src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp

@@ -1,5 +1,6 @@
 #include <gtest/gtest.h>
 #include <MUSTSensorDetection.h>
+#include <chrono>
 
 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<milliseconds>(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);
 }