diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index 06424d6ef..fdce595fc 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -57,7 +57,8 @@ sonar.modules= PedestrianPlugin, \ ERVCloudForwardingPlugin, \ CDASimAdapter, \ RSUHealthMonitorPlugin, \ - TelematicBridgePlugin + TelematicBridgePlugin, \ + MUSTSensorDriverPlugin @@ -86,6 +87,7 @@ ERVCloudForwardingPlugin.sonar.projectBaseDir =src/v2i-hub/ERVCloudForwa CDASimAdapter.sonar.projectBaseDir =src/v2i-hub/CDASimAdapter RSUHealthMonitorPlugin.sonar.projectBaseDir =src/v2i-hub/RSUHealthMonitorPlugin TelematicBridgePlugin.sonar.projectBaseDir =src/v2i-hub/TelematicBridgePlugin +MUSTSensorDriverPlugin.sonar.projectBaseDir =src/v2i-hub/MUSTSensorDriverPlugin @@ -125,6 +127,7 @@ RSUHealthMonitorPlugin.sonar.sources =src RSUHealthMonitorPlugin.sonar.exclusions =test/** TelematicBridgePlugin.sonar.sources =src TelematicBridgePlugin.sonar.exclusions =test/** +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. @@ -154,3 +157,4 @@ ERVCloudForwardingPlugin.sonar.tests=test CDASimAdapter.sonar.tests=test RSUHealthMonitorPlugin.sonar.tests=test TelematicBridgePlugin.sonar.tests=test +MUSTSensorDriverPlugin.sonar.tests=test diff --git a/src/tmx/Messages/include/SensorDetectedObject.h b/src/tmx/Messages/include/SensorDetectedObject.h new file mode 100644 index 000000000..d275bbf7e --- /dev/null +++ b/src/tmx/Messages/include/SensorDetectedObject.h @@ -0,0 +1,54 @@ +#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. + * 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 ) + + // 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; + + }; + + } + +}; // 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 179f48a13..000000000 --- a/src/tmx/Messages/include/simulation/SensorDetectedObject.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef INCLUDE_SIMULATED_SensorDetectedObject_H_ -#define INCLUDE_SIMULATED_SensorDetectedObject_H_ - -#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; - }; - - } - } - -}; // namespace tmx -#endif diff --git a/src/tmx/TmxUtils/src/UdpServer.cpp b/src/tmx/TmxUtils/src/UdpServer.cpp index 8299cd4e6..26155ac74 100644 --- a/src/tmx/TmxUtils/src/UdpServer.cpp +++ b/src/tmx/TmxUtils/src/UdpServer.cpp @@ -198,4 +198,22 @@ namespace tmx::utils { return -1; } + std::string UdpServer::stringTimedReceive(int maxWait_ms) { + std::vector msg(4000); + int num_of_bytes = this->TimedReceive(msg.data(),4000, maxWait_ms); + if (num_of_bytes > 0 ) { + msg.resize(num_of_bytes); + std::string ret(msg.data()); + FILE_LOG(logDEBUG) << "UDP Server message received : " << ret << " of size " << num_of_bytes << std::endl; + return ret; + } + else if ( num_of_bytes == 0 ) { + throw UdpServerRuntimeError("Received empty message!"); + } + else { + throw UdpServerRuntimeError("Listen timed out after 5 ms!"); + } + return ""; + } + } // namespace tmx::utils diff --git a/src/tmx/TmxUtils/src/UdpServer.h b/src/tmx/TmxUtils/src/UdpServer.h index bfb85f311..7c3b5664b 100644 --- a/src/tmx/TmxUtils/src/UdpServer.h +++ b/src/tmx/TmxUtils/src/UdpServer.h @@ -13,6 +13,7 @@ #include #include #include +#include namespace tmx { namespace utils { @@ -36,6 +37,8 @@ class UdpServer virtual int Receive(char *msg, size_t maxSize); virtual int TimedReceive(char *msg, size_t maxSize, int maxWait_ms); + virtual std::string stringTimedReceive(int maxWait_ms=5); + private: int _socket; int _port; diff --git a/src/tmx/TmxUtils/src/Vector3d.h b/src/tmx/TmxUtils/src/Vector3d.h new file mode 100644 index 000000000..6d4e9f184 --- /dev/null +++ b/src/tmx/TmxUtils/src/Vector3d.h @@ -0,0 +1,19 @@ +#pragma once + +namespace tmx::utils { + + + /// Three dimensional Vector + using Vector3d = 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; + }; + +} // 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..4ab7cabd7 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -132,7 +132,7 @@ namespace CDASimAdapter{ tmx::messages::TimeSyncMessage msg; msg.clear(); if (time_sync_listener) { - std::string str_msg = consume_server_message(time_sync_listener); + std::string str_msg = time_sync_listener->stringTimedReceive(); msg.set_contents( str_msg ); } else { @@ -142,13 +142,13 @@ 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) { - std::string str_msg = consume_server_message(sensor_detected_object_listener); + std::string str_msg = sensor_detected_object_listener->stringTimedReceive(); externalObj.set_contents(str_msg); } else @@ -180,23 +180,6 @@ namespace CDASimAdapter{ return ""; } - std::string CDASimConnection::consume_server_message( const std::shared_ptr _server) const { - std::vector msg(4000); - int num_of_bytes = _server->TimedReceive(msg.data(),4000, 5); - if (num_of_bytes > 0 ) { - msg.resize(num_of_bytes); - std::string ret(msg.data()); - PLOG(logDEBUG) << "UDP Server message received : " << ret << " of size " << num_of_bytes << std::endl; - return ret; - } - else if ( num_of_bytes == 0 ) { - throw UdpServerRuntimeError("Received empty message!"); - } - else { - throw UdpServerRuntimeError("Listen timed out after 5 ms!"); - } - return ""; - } std::string CDASimConnection::consume_v2x_message_from_simulation() const { if ( carma_simulation_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..3a9bdf1a4 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 @@ -64,12 +64,6 @@ namespace CDASimAdapter { * @param _client UDP client to forward message with. */ void forward_message(const std::string &v2x_message, const std::shared_ptr _client ) const ; - /** - * @brief Method to consume incoming std::string message from UDP Server. - * @param _server UDP Server to consume string message from. - * @return string of message. - */ - std::string consume_server_message( const std::shared_ptr _server ) const; /** * @brief Method to consume incoming std::string message in hex format from UDP Server. @@ -88,7 +82,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/CDASimAdapter/test/TestCDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCDASimConnection.cpp index c1ec10d4d..710dc49d8 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCDASimConnection.cpp @@ -59,25 +59,6 @@ namespace CDASimAdapter { connection->forward_message(test_message, client); } - TEST_F( TestCDASimConnection, consume_msg){ - - std::shared_ptr server = std::make_shared(); - char *msg_data = new char(); - char test_string[] = "Test Message"; - EXPECT_CALL( *server, TimedReceive(_, _, _) ).Times(2). - WillOnce(testing::DoAll(Return(-1))). - WillRepeatedly( testing::DoAll( SetArrayArgument<0>(test_string, test_string + strlen(test_string) + 1),Return(10))); - ASSERT_THROW(connection->consume_server_message(server), UdpServerRuntimeError); - - std::string msg = connection->consume_server_message(server); - - std::string compare_str; - compare_str = test_string; - ASSERT_EQ(compare_str.compare( msg ) , 0); - delete msg_data; - - } - TEST_F( TestCDASimConnection, setup_upd_connection) { ASSERT_TRUE(connection->setup_udp_connection("127.0.0.1", "127.0.0.1", 4567, 4568, 4569, 4570)); } diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/CMakeLists.txt b/src/v2i-hub/MUSTSensorDriverPlugin/CMakeLists.txt new file mode 100755 index 000000000..8d21886ed --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/CMakeLists.txt @@ -0,0 +1,27 @@ +PROJECT(MUSTSensorDriverPlugin VERSION 7.6.0 LANGUAGES CXX) +set(CMAKE_CXX_STANDARD 17) + +set(TMX_PLUGIN_NAME "Must Sensor Driver Plugin") + + +BuildTmxPlugin() + +TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC tmxutils ) + +############# +## Testing ## +############# +enable_testing() +add_library(${PROJECT_NAME}_lib src/MUSTSensorDetection.cpp) +TARGET_LINK_LIBRARIES(${PROJECT_NAME}_lib PUBLIC tmxutils ) + +set(BINARY ${PROJECT_NAME}_test) +file(GLOB_RECURSE TEST_SOURCES LIST_DIRECTORIES false test/*.h test/*.cpp) +set(SOURCES ${TEST_SOURCES} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) +add_executable(${BINARY} ${TEST_SOURCES}) +add_test(NAME ${BINARY} COMMAND ${BINARY}) +TARGET_INCLUDE_DIRECTORIES(${BINARY} PUBLIC /usr/local/lib src/) + +target_link_libraries(${BINARY} PUBLIC + ${PROJECT_NAME}_lib + gtest) \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/README.md b/src/v2i-hub/MUSTSensorDriverPlugin/README.md new file mode 100644 index 000000000..fd607eb26 --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/README.md @@ -0,0 +1,80 @@ +# MUST Sensor Driver Plugin Documentation + +## Introduction + +[MUST](https://www.aiwaysion.com/technology) (Mobile Unit for Sensing Traffic) Sensor from AI Waysion is a camera based sensor, planned to be used for cooperative perception in freight use cases. The MUST Sensor provides detections via UDP packets made up of CSV (Comma Separated Values) string data. The V2X-Hub MUST Sensor Driver Plugin will then consume these messages and translate them to **Sensor Detected Object** messages, which is V2X-Hub's generic detection message. This message is consumable by the **CARMA Streets [Sensor Data Sharing Service](https://github.com/usdot-fhwa-stol/carma-streets/blob/develop/sensor_data_sharing_service/README.md)** which will generate **Sensor Data Sharing Message**s according to the J3224 standard for broadcast to other traffic actors in the area. + +## Related Plugins + +A list of plugins related to the MUST Sensor Driver Plugin. + +### Immediate Forward Plugin + +For RSU Immediate Message Forwarding (IMF) functionality forward SDSMs (Sensor Data Sharing Message). + +### CARMA Streets Plugin + +For forwarding detection data (SensorDetectedObject) to **[Sensor Data Sharing Service](https://github.com/usdot-fhwa-stol/carma-streets/blob/develop/sensor_data_sharing_service/README.md)** for creation of SDSMs. + +## Configuration/Deployment + +This plugin has several configuration parameters. Below these are listed out as together with descriptions on how to set them. + +**DetectionReceiverIp**: This is the IP address on which V2X-Hub will listen for detections. In most scenarios this can be left at default since 127.0.0.1 should resolve to the IP address of V2X-Hub. + +**DetectionReceiverPort**: This is the Port on which V2X-Hub will list for detection. In most scenarios this can be left at default as well. The MUST Sensor must be configured to broadcast it's detection information to the configured IP and Port. + +**SensorId**: This is a unique string identifier for this sensor. Multiple instances of MUST Sensors can be connected via multiple instances of this plugin. Additionally other sensors can also be connected to V2X-Hub conccurently. For cooperative perception functionality to work correctly, each of these sensors must have a unique string identifier. + +> [!NOTE] +> V2X-Hub currently has no mechanism by which to verify that all configured sensors have unique string indentifies. Currently this can only be verified via manual inspection. + +**ProjectionString**: This parameter holds a string that describes coordinate transforms necessary for projecting the detection data provide in cartesian coordinates to WSG84 coordinate frame. + +> [!NOTE] +> Both **CARMA Streets** and our vehicle automatation **CARMA Platform** rely on the PROJ4 library for projecting data between internal local maps coordinate frames and WSG84. Additional documentation on the projection string can be found in PROJ documentation ()()() + +After setting these configuration parameters the plugin can simply be enabled. + +## Design + +![Alt text](docs/communication_diagram.png) +This plugin consists of a simple UDP Server listening for detection data from the MUST Sensor. Each received detection is deserialized and translated to a **Sensor Detected Object** message. Then this **Sensor Detected Object** message is forward on the TMX Message bus. If enabled, the **CARMA Streets Plugin** will receive this message, forward it to the **CARMA Streets [Sensor Data Sharing Service](https://github.com/usdot-fhwa-stol/carma-streets/blob/develop/sensor_data_sharing_service/README.md)** which is responsible for generating SDSMs from detection data. These SDSMs are sent back to V2X-Hub for broadcast to vehicle's via the RSU (Road Side Unit). + +### Coordinate Frame Translation + +MUST Sensor produces detection data as CSV Strings. The detection data includes position as a 2 dimensional cartesian offset (in meters) from the sensor location. Velocity is provided using a NE (yNorth xEast) heading (in degrees) and a speed (in m/s). This needs to be translated to an ENU (xEast,yNorth,zUp) cartesian cordinate position and velocity vector. The position does not need any translation since both of xEast yNorth. The heading and speed must be translated to a velocity vector using trigonometry. +![Alt text](docs/sensor_coordinate_frame.png) + +![Alt text](docs/heading.png) + +To convert the heading to unit circle we simple subtract 270 degrees from any heading value, then we can take the `cos()` for x values and the `sin()` for y values. + +![Alt text](docs/unit_circle.png) + +### Messages + +**Sensor Detected Object**: V2X-Hub's generic message for detection data. + +## Functionality Testing + +Included in this directory is a script that can be used to provide Mock MUST Sensor data. Start up plugin and use `scripts/MockMUSTSensor.py` script to send mock detection data from a single mock object to plugin at 30 Hz. The script will update the timestamp of this object as well as move it randomly in space. + +```bash +Script to mock detection data coming from MUST Sensor + +options: + -h, --help show this help message and exit + --ip IP IP address to send detection data to. + --port PORT Port to send detection data to. +``` + +Addition addition objects for which to send mock detections can be done by appending detections to the `detections` array in the python script. + +### Confirming Functionality + +On startup of the plugin, the V2X-Hub Web UI should show the plugin as enabled and the `MUST Sensor Connection Status` should be `IDLE` reflecting that the plugin is listening for detections but has not yet received any. +![Alt text](docs/idle_connection.png) +After running the `MockMUSTSensor.py` script the `MUST Sensor Connection Status` should be `CONNECTED` reflecting that it is currently receiving valid messages from the MUST Sensor. Additionally the **Messages** tab will reveal that the plugin is also sending **SensorDetectedObject** messages as a result. +![Alt text](docs/connected_connection.png) +A status of `DISCONNECTED` represents some error behavior that is likely related to the connection or the incomming data. Please inspect configuration parameters on the MUST Plugin and the MUST Sensor Detection data and confirm it is valid \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/docs/communication_diagram.png b/src/v2i-hub/MUSTSensorDriverPlugin/docs/communication_diagram.png new file mode 100644 index 000000000..f4c292378 Binary files /dev/null and b/src/v2i-hub/MUSTSensorDriverPlugin/docs/communication_diagram.png differ diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/docs/connected_connection.png b/src/v2i-hub/MUSTSensorDriverPlugin/docs/connected_connection.png new file mode 100644 index 000000000..c61029e0c Binary files /dev/null and b/src/v2i-hub/MUSTSensorDriverPlugin/docs/connected_connection.png differ diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/docs/heading.png b/src/v2i-hub/MUSTSensorDriverPlugin/docs/heading.png new file mode 100644 index 000000000..335d7cc23 Binary files /dev/null and b/src/v2i-hub/MUSTSensorDriverPlugin/docs/heading.png differ diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/docs/idle_connection.png b/src/v2i-hub/MUSTSensorDriverPlugin/docs/idle_connection.png new file mode 100644 index 000000000..c0134406a Binary files /dev/null and b/src/v2i-hub/MUSTSensorDriverPlugin/docs/idle_connection.png differ diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/docs/sensor_coordinate_frame.png b/src/v2i-hub/MUSTSensorDriverPlugin/docs/sensor_coordinate_frame.png new file mode 100644 index 000000000..00ceb6be7 Binary files /dev/null and b/src/v2i-hub/MUSTSensorDriverPlugin/docs/sensor_coordinate_frame.png differ diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/docs/unit_circle.png b/src/v2i-hub/MUSTSensorDriverPlugin/docs/unit_circle.png new file mode 100644 index 000000000..fbd570c96 Binary files /dev/null and b/src/v2i-hub/MUSTSensorDriverPlugin/docs/unit_circle.png differ diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json new file mode 100755 index 000000000..dcd15c60f --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json @@ -0,0 +1,42 @@ +{ + "name": "MUSTSensorDriver", + "description": "Plugin for processing MUST Sensor Data.", + "version": "@PROJECT_VERSION@", + "exeLocation": "/bin/MUSTSensorDriverPlugin", + "coreIpAddr":"127.0.0.1", + "corePort":24601, + "messageTypes": [ + { + "type": "Application", + "subtype": "SensorDetectedObject", + "description": "Generic message for detection from Sensor." + } + ], + "configuration": [ + { + "key": "LogLevel", + "default": "INFO", + "description": "The log level for this plugin" + }, + { + "key":"DetectionReceiverIP", + "default":"127.0.0.1", + "description":"IP Address V2X-Hub listens for incoming detections" + }, + { + "key":"DetectionReceiverPort", + "default":"4545", + "description":"Port V2X-Hub listens for incoming detections" + }, + { + "key":"SensorId", + "default":"MUSTSensor1", + "description":"Unique Idenifier for Sensor" + }, + { + "key":"ProjectionString", + "default":"+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", + "description":"Projection string for projecting cartesian detection data into WSG84 coordinates." + } + ] +} \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/scripts/MockMUSTSensor.py b/src/v2i-hub/MUSTSensorDriverPlugin/scripts/MockMUSTSensor.py new file mode 100755 index 000000000..4d3837861 --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/scripts/MockMUSTSensor.py @@ -0,0 +1,83 @@ +#!/usr/bin/python3 + +import socket +import time +import argparse +from dataclasses import dataclass +from enum import Enum +import random + + +class DetectionClassification(Enum): + """Enumeration used for identifying type of detection + """ + SEDAN='sedan' + VAN='van' + TRUCK='truck' +class DetectionSize(Enum): + """Enumeration used for identifying the type of KafkaLogMessage + """ + SMALL='small' + MEDIUM='medium' + LARGE='large' + +@dataclass +class MUSTDetection: + """Class used to store data for each Kafka Log Message + """ + classification: DetectionClassification + x: float + y: float + heading: float + speed: float + size: DetectionSize + confidence: float + track_id: int + timestamp: int + + def to_csv(self): + return f'{self.classification.value},{self.x},{self.y},{self.heading},{self.speed},{self.size.value},{self.confidence},{self.track_id},{self.timestamp}' + +def update_detection(detection): + """Function moves detection and heading by random increment and changes speed to a random value between 0 and 10. + """ + detection.x = random.uniform(-1.0, 1.0) + detection.x + detection.y = random.uniform(-1.0, 1.0) + detection.y + detection.heading = random.uniform(-5.0, 5.0) + detection.heading + detection.speed = random.uniform(0.0, 10) + detection.timestamp = round(time.time()) + + +def create_socket(): + return socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM) + +def send_detection(sock, detection, host): + try: + msg = detection.to_csv() + encoded_msg = str.encode(msg) + sock.sendto(encoded_msg,host) + print( encoded_msg.decode(encoding= 'UTF-8'), 'was sent to ', host) + except socket.gaierror: + print ('There an error resolving the host') + +detections = [] +detections.append(MUSTDetection(DetectionClassification.SEDAN, 0, 0, 330, 1, DetectionSize.MEDIUM, 95, 2,round(time.time()))) + +def main(): + parser = argparse.ArgumentParser(description='Script to mock detection data coming from MUST Sensor') + parser.add_argument('--ip', help='IP address to send detection data to.', type=str, default="127.0.0.1") + parser.add_argument('--port', help='Port to send detection data to.', type=str, default=4545) + args = parser.parse_args() + sock = create_socket() + host = (args.ip, args.port) + + print("Mocking MUST Sensor detections ...") + while True: + for detection in detections: + update_detection(detection) + send_detection(sock,detection,host) + # MUST Sensor broadcasts detection data at 30 Hz + time.sleep(0.0333333) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp new file mode 100644 index 000000000..95b514d9e --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp @@ -0,0 +1,91 @@ +#include "MUSTSensorDetection.h" + +namespace MUSTSensorDriverPlugin { + + + MUSTSensorDetection csvToDetection(const std::string &csv ) { + MUSTSensorDetection detection; + std::vector csv_values; + std::stringstream ss(csv); + while (ss.good()) { + std::string substr; + std::getline(ss, substr, ','); + csv_values.push_back(substr); + } + if (csv_values.size() != 9 ){ + FILE_LOG(tmx::utils::logERROR) << "Data " << csv << " does not match expected csv data format : \'class,x,y,heading,speed,size,confidence,trackId,timestamp\'" << std::endl; + throw tmx::TmxException("Failed to parse CSV MUST Detection data"); + } + // Read out CSV information + detection.cl = fromStringToDetectionClassification(csv_values.at(0)); + detection.position_x = std::stod(csv_values.at(1)); + detection.position_y = std::stod(csv_values.at(2)); + detection.heading = std::stod(csv_values.at(3)); + detection.speed = std::stod(csv_values.at(4)); + detection.size = fromStringToDetectionSize(csv_values.at(5)); + detection.confidence = std::stod(csv_values.at(6)); + detection.trackID = std::stoi(csv_values.at(7)); + detection.timestamp = std::stod(csv_values.at(8)); + return detection; + } + + 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; + return detectedObject; + } + DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept { + try { + + return stringToDetectionClassificationMap.at(str); + } + catch( const std::out_of_range &e ) { + FILE_LOG(tmx::utils::logWARNING) << e.what() << "No registered Detection Classification for " << str << " in stringToDetectionClassificationMap! Setting classification as NA." << std::endl; + return DetectionClassification::NA; + } + } + + std::string detectionClassificationToSensorDetectedObjectType(const DetectionClassification &classification) { + for (auto const &[name, cl] : stringToDetectionClassificationMap){ + if (classification == cl) { + std::string rtn = name; + std::transform(rtn.begin(), rtn.end(), rtn.begin(), ::toupper); + return rtn; + } + } + throw tmx::TmxException("DetectionClassification type is not registered in stringToDetectionClassificationMap!"); + } + + + + DetectionSize fromStringToDetectionSize(const std::string &str) noexcept { + try { + + return stringToDetectionSizeMap.at(str); + } + catch( const std::out_of_range &e) { + FILE_LOG(tmx::utils::logWARNING) << e.what() << "No registered Detection Size for " << str << " in stringToDetectionSizeMap! Setting classification as NA." << std::endl; + return DetectionSize::NA; + } + }; + + tmx::utils::Vector3d 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; + return velocity; + }; +} diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h new file mode 100644 index 000000000..313204811 --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h @@ -0,0 +1,122 @@ +#pragma once +#include +#include +#include // std::out_of_range +#include +#include +#include +#include +#include + + +namespace MUSTSensorDriverPlugin { + /** + * @brief Enumeration for Detection Classifications + */ + enum class DetectionClassification { + SEDAN, + TRUCK, + VAN, + NA + }; + + /** + * @brief Enumeration for Detection Sizes + */ + enum class DetectionSize + { + SMALL, + MEDIUM, + LARGE, + NA + }; + + /** + * @brief Static map used to convert string detection size information to enumeration. + */ + const static std::unordered_map stringToDetectionSizeMap = { + { "small", DetectionSize::SMALL}, + { "medium", DetectionSize::MEDIUM}, + { "large", DetectionSize::LARGE} + }; + + /** + * @brief Static map used to convert string detection classification to enumeration. + */ + const static std::unordered_map stringToDetectionClassificationMap = { + {"sedan", DetectionClassification::SEDAN}, + {"truck", DetectionClassification::TRUCK}, + {"van", DetectionClassification::VAN} + }; + + /** + * @brief Function to convert string detection size information to enumeration. + * @param str detection size + * @return DetectionSize enumeration if found in map or DetectionSize::NA if not found. + */ + DetectionSize fromStringToDetectionSize(const std::string &str) noexcept; + + /** + * @brief Function to convert string detection classification information to enumeration. + * @param str detection classification + * @return DetectionClassification enumeration if found in map or DetectionClassification::NA if not found. + */ + DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept; + + /** + * @brief Converts DetectionClassification enumeration to string type for SensorDetectedObject. All types are + * assumed to be capitalize versions of the DetectionClassifications. + * @param classifcation DetectionClassification + * @return std::string type for SensorDetectedObject + * @throws tmx::TmxException if DetectionClassification is not included in map. + */ + std::string detectionClassificationToSensorDetectedObjectType(const DetectionClassification &classifcation); + + /** + * @brief Struct for storing MUST Sensor Detection information + */ + struct MUSTSensorDetection { + DetectionClassification cl = DetectionClassification::NA; + // Meters + double position_x = 0; + // Meters + double position_y = 0; + // Degrees + double heading = 0; + // Meters/Second + double speed = 0; + DetectionSize size = DetectionSize::NA; + // Confidence in type + double confidence = 0; + // Unique ID + unsigned int trackID = 0; + // Timestamp in seconds + double timestamp = 0; + + }; + + /** + * @brief Function to convert CSV string to MUSTSensorDetection struct + * @param csv std::string + * @return MUSTSensorDetection + * @throws tmx::TmxException if string is misformatted. + */ + MUSTSensorDetection csvToDetection(const std::string &csv ); + + /** + * @brief Function to convert MUSTSensorDetections to SensorDetectedObject + * @param detection MUSTSensorDetection + * @param sensorId std::string unique indentifier of MUSTSensor + * @param projString std::string describing reference point and WGS84 projection of detection information + * @return tmx::messages::SensorDetectedObject + */ + tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString); + + /** + * @brief Function to convert MUSTSensor provided heading and speed to a velocity vector + * @param heading double heading in degrees + * @param speed double speed in m/s + * @return tmx::utils::Vector3d velocity. + */ + 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 new file mode 100644 index 000000000..946d67869 --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp @@ -0,0 +1,117 @@ +/** + * Copyright (C) 2024 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ +#include "MUSTSensorDriverPlugin.h" + +using namespace tmx::utils; +using namespace std; + +namespace MUSTSensorDriverPlugin { + + MUSTSensorDriverPlugin::MUSTSensorDriverPlugin(const string &name): PluginClientClockAware(name) + { + mustSensorPacketReceiverThread = std::make_unique(std::chrono::milliseconds(5)); + if (PluginClientClockAware::isSimulationMode()) { + PLOG(tmx::utils::logINFO) << "Simulation mode on " << std::endl; + SubscribeToMessages(); + } + } + + void MUSTSensorDriverPlugin::OnStateChange(IvpPluginState state) { + PluginClientClockAware::OnStateChange(state); + if (state == IvpPluginState_registered) { + UpdateConfigSettings(); + }else { + connected = false; + SetStatus(keyMUSTSensorConnectionStatus, "DISCONNECTED"); + } + } + + void MUSTSensorDriverPlugin::UpdateConfigSettings() + { + // Configuration settings are retrieved from the API using the GetConfigValue template class. + // This method does NOT execute in the main thread, so variables must be protected + // (e.g. using std::atomic, std::mutex, etc.). + if (this->IsPluginState(IvpPluginState_registered)) { + std::scoped_lock lock(_configMutex); + GetConfigValue("ProjectionString", projString); + GetConfigValue("SensorId", sensorId); + // Setup UDP Server + std::string ip_address; + unsigned int port; + GetConfigValue("DetectionReceiverIP", ip_address); + GetConfigValue("DetectionReceiverPort", port); + createUdpServer(ip_address, port); + SetStatus(keyMUSTSensorConnectionStatus, "IDLE"); + + mustSensorPacketReceiverThreadId = mustSensorPacketReceiverThread->AddPeriodicTick([this]() { + this->processMUSTSensorDetection(); + + } // end of lambda expression + , std::chrono::milliseconds(5) ); + mustSensorPacketReceiverThread->Start(); + } + } + void MUSTSensorDriverPlugin::processMUSTSensorDetection(){ + if (mustSensorPacketReceiver) { + try { + PLOG(logDEBUG1) << "Processing MUST Sensor Detection ... " << std::endl; + MUSTSensorDetection detection = csvToDetection(mustSensorPacketReceiver->stringTimedReceive()); + if ( !connected ) { + connected = true; + SetStatus(keyMUSTSensorConnectionStatus, "CONNECTED"); + } + tmx::messages::SensorDetectedObject msg = mustDetectionToSensorDetectedObject(detection, sensorId, projString); + PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl; + this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); + } + catch( const tmx::utils::UdpServerRuntimeError &e) { + PLOG(logERROR) << "Error occurred processing MUSTSensorDetection" << e << std::endl; + SetStatus(keyMUSTSensorConnectionStatus, "DISCONNECTED"); + connected = false; + } + catch ( const tmx::TmxException &e){ + PLOG(logERROR) << "Error occurred processing MUSTSensorDetection" << e.what() << std::endl; + SetStatus(keyMUSTSensorConnectionStatus, "DISCONNECTED"); + connected = false; + } + }else { + SetStatus(keyMUSTSensorConnectionStatus, "DISCONNECTED"); + connected = false; + } + } + + void MUSTSensorDriverPlugin::createUdpServer(const std::string &address, unsigned int port) { + if ( mustSensorPacketReceiver ) { + mustSensorPacketReceiver.reset(new UdpServer(address, port)); + } + else { + mustSensorPacketReceiver = std::make_unique(address, port); + } + } + + void MUSTSensorDriverPlugin::OnConfigChanged(const char *key, const char *value) + { + PluginClientClockAware::OnConfigChanged(key, value); + UpdateConfigSettings(); + } + + +} /* namespace MUSTSensorDriver */ + +int main(int argc, char *argv[]) +{ + return run_plugin("MUSTSensorDriverPlugin", argc, argv); +} diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h new file mode 100644 index 000000000..3d9e33d8c --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h @@ -0,0 +1,73 @@ +/** + * Copyright (C) 2019 LEIDOS. + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this except in compliance with the License. You may obtain a copy of + * the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations under + * the License. + */ +#pragma once + +#include +#include +#include +#include +#include + +#include "MUSTSensorDetection.h" + + + + +namespace MUSTSensorDriverPlugin +{ + /** + * @brief TODO Plugin description + */ + class MUSTSensorDriverPlugin : public tmx::utils::PluginClientClockAware + { + private: + std::mutex _configMutex; + /** + * @brief Status label simulation time to be displayed by each plugin. + */ + const char* keyMUSTSensorConnectionStatus = "MUST Sensor Connection Status"; + + std::unique_ptr mustSensorPacketReceiver; + + std::unique_ptr mustSensorPacketReceiverThread; + + std::string sensorId; + + std::string projString; + + bool connected = false; + + // Message receiver thread id + int mustSensorPacketReceiverThreadId; + /** + * @brief Callback triggered on configuration updates + */ + void UpdateConfigSettings(); + void OnConfigChanged(const char *key, const char *value) override; + void createUdpServer(const std::string &address, unsigned int port); + void OnStateChange(IvpPluginState state) override; + void processMUSTSensorDetection(); + + public: + /** + * @brief Constructor + * @param name Plugin Name + */ + explicit MUSTSensorDriverPlugin(const std::string &name); + + }; + +} \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp new file mode 100644 index 000000000..a50783d3a --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp @@ -0,0 +1,89 @@ +#include +#include +#include + +using namespace MUSTSensorDriverPlugin; + +TEST(TestMUSTSensorDetection, fromStringToDetectionSize) +{ + EXPECT_EQ(DetectionSize::SMALL, fromStringToDetectionSize("small")); + EXPECT_EQ(DetectionSize::MEDIUM, fromStringToDetectionSize("medium")); + EXPECT_EQ(DetectionSize::LARGE, fromStringToDetectionSize("large")); + EXPECT_EQ(DetectionSize::NA, fromStringToDetectionSize("not_a_size")); + +} + +TEST(TestMUSTSensorDetection, fromStringToDetectionClassification) +{ + EXPECT_EQ(DetectionClassification::SEDAN, fromStringToDetectionClassification("sedan")); + EXPECT_EQ(DetectionClassification::VAN, fromStringToDetectionClassification("van")); + EXPECT_EQ(DetectionClassification::TRUCK, fromStringToDetectionClassification("truck")); + EXPECT_EQ(DetectionClassification::NA, fromStringToDetectionClassification("not_a_classification")); + +} + +TEST(TestMUSTSensorDetection, csvToDetection ){ + std::string valid_csv_data = "truck,13.3,22.4,30.5,35.8,large,95.1,1,1714374738"; + auto detection = csvToDetection(valid_csv_data); + EXPECT_EQ(detection.cl, DetectionClassification::TRUCK); + EXPECT_DOUBLE_EQ(detection.position_x, 13.3); + EXPECT_DOUBLE_EQ(detection.position_y, 22.4); + EXPECT_DOUBLE_EQ(detection.heading, 30.5); + EXPECT_DOUBLE_EQ(detection.speed, 35.8); + EXPECT_EQ(detection.size, DetectionSize::LARGE); + EXPECT_DOUBLE_EQ(detection.confidence, 95.1); + EXPECT_EQ(detection.trackID, 1); + EXPECT_EQ(detection.timestamp, 1714374738); +} + +TEST(TestMUSTSensorDetection, csvToDectectionInvalidCSV ){ + std::string invalid_csv_data = "truck,13.3,22.4,30.5,35.8,large,95.1,1,1714374738,20"; + EXPECT_THROW(csvToDetection(invalid_csv_data), tmx::TmxException); +} + +TEST(TestMUSTSensorDetection, csvToDectectionEmptyString ){ + std::string empty_string = ""; + EXPECT_THROW(csvToDetection(empty_string), tmx::TmxException); +} + +TEST(TestMUSTSensorDetection, mustDetectionToSensorDetectedObject ) { + 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 = 1719506355.4; + detection.trackID = 324; + detection.speed = 5; + + 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); +} + +TEST(TestMUSTSensorDetection, detectionClassificationToSensorDetectedObjectType ) { + EXPECT_STRCASEEQ("SEDAN", detectionClassificationToSensorDetectedObjectType(DetectionClassification::SEDAN).c_str()); + EXPECT_STRCASEEQ("VAN", detectionClassificationToSensorDetectedObjectType(DetectionClassification::VAN).c_str()); + EXPECT_STRCASEEQ("TRUCK", detectionClassificationToSensorDetectedObjectType(DetectionClassification::TRUCK).c_str()); + EXPECT_THROW(detectionClassificationToSensorDetectedObjectType(DetectionClassification::NA).c_str(), std::runtime_error); + +} + +TEST(TestMUSTSensorDetection, headingSpeedToVelocity ) { + auto velocity = headingSpeedToVelocity(30, 5); + EXPECT_NEAR(4.33, velocity.Y, 0.001); + EXPECT_NEAR(-2.5, velocity.X, 0.001); +} \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/main.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/main.cpp new file mode 100644 index 000000000..ba7cd2667 --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/test/main.cpp @@ -0,0 +1,8 @@ + +#include + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}