diff --git a/src/tmx/TmxUtils/src/Vector3d.h b/src/tmx/TmxUtils/src/Vector3d.h index d8e6a1dfb..6d4e9f184 100644 --- a/src/tmx/TmxUtils/src/Vector3d.h +++ b/src/tmx/TmxUtils/src/Vector3d.h @@ -4,7 +4,7 @@ namespace tmx::utils { /// Three dimensional Vector - typedef struct Vector3d + using Vector3d = struct Vector3d { Vector3d() : X(0), Y(0), Z(0) {} @@ -14,6 +14,6 @@ namespace tmx::utils { double X; double Y; double Z; - } Vector3d; + }; } // namespace tmx::utils diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 4e6d08ab4..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 { @@ -148,7 +148,7 @@ namespace CDASimAdapter{ 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/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index b79cd3534..3a9bdf1a4 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -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. 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/README.md b/src/v2i-hub/MUSTSensorDriverPlugin/README.md new file mode 100644 index 000000000..bb2e135c9 --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/README.md @@ -0,0 +1,49 @@ +# MUST Sensor Driver Plugin Documentation + +## Introduction + +MUST (Mobile Unit for Sensing Traffic) Sensor is from AI Waysion described in more detail [here](https://www.aiwaysion.com/technology).This is a camera based sensor that is 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** 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** 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 instance of this plugin. Additionally other sensor can also be connected to V2X-Hub conccurently. For cooperative perception functionality to work correctly, each of thes sensor 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 (https://proj.org/en/9.4/index.html)(https://proj.org/en/9.4/usage/quickstart.html)(https://proj.org/en/9.4/usage/transformation.html) + +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 **Sensor Detected Object** data via free functions. Then this **Sensor Detected Object** 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). + +### Messages + +**Sensor Detected Object**: V2X-Hub's generic message for detection data. + +## Functionality Testing + 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/manifest.json b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json index 771e63149..dcd15c60f 100755 --- a/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json +++ b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json @@ -1,22 +1,23 @@ { - "name": "RSUHealthMonitor", - "description": "Monitor RSU health status", + "name": "MUSTSensorDriver", + "description": "Plugin for processing MUST Sensor Data.", "version": "@PROJECT_VERSION@", - "exeLocation": "/bin/RSUHealthMonitorPlugin", + "exeLocation": "/bin/MUSTSensorDriverPlugin", "coreIpAddr":"127.0.0.1", "corePort":24601, - "messageTypes": [], + "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":"Interval", - "default":"1", - "description": "Sending RSU SNMP GET request at every configured interval. Default every 1 second. Unit of measure: second." - }, { "key":"DetectionReceiverIP", "default":"127.0.0.1", diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/scripts/MockMUSTSensor.py b/src/v2i-hub/MUSTSensorDriverPlugin/scripts/MockMUSTSensor.py new file mode 100644 index 000000000..e35efeb9f --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/scripts/MockMUSTSensor.py @@ -0,0 +1,75 @@ +#!/usr/bin/python3 + +import socket +import sys +import time +import argparse +from dataclasses import dataclass +from enum import Enum + + +class DetectionClassification(Enum): + """Enumeration used for indentifying type of detection + """ + SEDAN='sedan' + VAN='van' + TRUCK='truck' +class DetectionSize(Enum): + """Enumeration used for indentifying 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(): + return f'{self.classification},{self.x},{self.y},{self.heading},{self.speed},{self.size},{self.confidence},{self.track_id},{self.timstamp}' + +def move_detection(): + return + +def create_socket(): + try: + return socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM) + except socket.error as err: + print('Socket error because of %s' %(err)) +def send_detection(sock, detection, host): + try: + msg = detection + 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() * 1000))) + +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: + send_detection(sock,detection,host) + +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 index 07f45ae7b..d23ee4d98 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp @@ -29,7 +29,7 @@ namespace MUSTSensorDriverPlugin { return detection; } - tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, const std::string &sensorId, const std::string &projString) { + 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; @@ -47,7 +47,8 @@ namespace MUSTSensorDriverPlugin { return stringToDetectionClassificationMap.at(str); } - catch( const std::out_of_range &e) { + catch( const std::out_of_range ) { + FILE_LOG(tmx::utils::logWARNING) << "No registered Detection Classification for " << str << " in stringToDetectionClassificationMap! Setting classification as NA." << std::endl; return DetectionClassification::NA; } } @@ -70,7 +71,8 @@ namespace MUSTSensorDriverPlugin { return stringToDetectionSizeMap.at(str); } - catch( const std::out_of_range &e) { + catch( const std::out_of_range ) { + FILE_LOG(tmx::utils::logWARNING) << "No registered Detection Size for " << str << " in stringToDetectionSizeMap! Setting classification as NA." << std::endl; return DetectionSize::NA; } }; diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h index c5fb00749..74352c1bb 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h @@ -59,7 +59,7 @@ namespace MUSTSensorDriverPlugin { MUSTSensorDetection csvToDectection(const std::string &csv ); - tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, const std::string &sensorId, const std::string &projString); + tmx::messages::SensorDetectedObject mustDetectionToSensorDetectedObject(const MUSTSensorDetection &detection, std::string_view sensorId, std::string_view projString); 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 8e51474ca..527c2218c 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp @@ -44,8 +44,11 @@ namespace MUSTSensorDriverPlugin { GetConfigValue("DetectionReceiverIP", ip_address); GetConfigValue("DetectionReceiverPort", port); createUdpServer(ip_address, port); - auto message_receiver_tick_id = mustSensorPacketReceiverThread->AddPeriodicTick([this]() { + SetStatus(keyMUSTSensorConnectionStatus, "IDLE", true); + + mustSensorPacketReceiverThreadId = mustSensorPacketReceiverThread->AddPeriodicTick([this]() { this->processMUSTSensorDetection(); + } // end of lambda expression , std::chrono::milliseconds(5) ); mustSensorPacketReceiverThread->Start(); @@ -53,10 +56,23 @@ namespace MUSTSensorDriverPlugin { } void MUSTSensorDriverPlugin::processMUSTSensorDetection(){ if (mustSensorPacketReceiver) { - MUSTSensorDetection detection = csvToDectection(mustSensorPacketReceiver->stringTimedReceive()); - tmx::messages::SensorDetectedObject msg = mustDetectionToSensorDetectedObject(detection, sensorId, projString); - PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl; - this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); + try { + MUSTSensorDetection detection = csvToDectection(mustSensorPacketReceiver->stringTimedReceive()); + if ( !connected ) { + connected = true; + SetStatus(keyMUSTSensorConnectionStatus, "CONNECTED", true); + } + 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) { + SetStatus(keyMUSTSensorConnectionStatus, "DISCONNECTED", true); + connected = false; + } + }else { + SetStatus(keyMUSTSensorConnectionStatus, "DISCONNECTED", true); + connected = false; } } diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h index 66985190d..fd37dfafa 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h @@ -38,7 +38,7 @@ namespace MUSTSensorDriverPlugin /** * @brief Status label simulation time to be displayed by each plugin. */ - const char* keySensorConnectionStatus = "Sensor Connection Status"; + const char* keyMUSTSensorConnectionStatus = "MUST Sensor Connection Status"; std::unique_ptr mustSensorPacketReceiver; @@ -47,6 +47,11 @@ namespace MUSTSensorDriverPlugin std::string sensorId; std::string projString; + + bool connected = false; + + // Message receiver thread id + int mustSensorPacketReceiverThreadId; /** * @brief Callback triggered on configuration updates */