diff --git a/src/tmx/TmxUtils/src/UdpServer.cpp b/src/tmx/TmxUtils/src/UdpServer.cpp index 8299cd4e6..e5e157d78 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() { + std::vector msg(4000); + int num_of_bytes = this->TimedReceive(msg.data(),4000, 5); + 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..db41f3fed 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(); + private: int _socket; int _port; diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/CMakeLists.txt b/src/v2i-hub/MUSTSensorDriverPlugin/CMakeLists.txt index 717ec6b71..358988482 100755 --- a/src/v2i-hub/MUSTSensorDriverPlugin/CMakeLists.txt +++ b/src/v2i-hub/MUSTSensorDriverPlugin/CMakeLists.txt @@ -1,4 +1,5 @@ PROJECT(MUSTSensorDriverPlugin VERSION 7.6.0 LANGUAGES CXX) +set(CMAKE_CXX_STANDARD 17) set(TMX_PLUGIN_NAME "Must Sensor Driver Plugin") @@ -11,16 +12,15 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC tmxutils ) ## Testing ## ############# enable_testing() -# add_library(${PROJECT_NAME}_lib ) -# target_link_libraries(${PROJECT_NAME}_lib PUBLIC -# tmxutils - -# ) +add_library(${PROJECT_NAME}_lib src/MUSTSensorDetection.cpp) + 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 + ${PROJECT_NAME}_lib gtest) \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json index c96601e69..771e63149 100755 --- a/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json +++ b/src/v2i-hub/MUSTSensorDriverPlugin/manifest.json @@ -18,9 +18,24 @@ "description": "Sending RSU SNMP GET request at every configured interval. Default every 1 second. Unit of measure: second." }, { - "key":"RSUConfigurationList", - "default":"{ \"RSUS\": [ { \"RSUIp\": \"192.168.XX.XX\", \"SecurityLevel\":\"authPriv\", \"SNMPPort\": \"161\", \"AuthPassPhrase\": \"dummy\", \"User\": \"authOnlyUser\", \"RSUMIBVersion\": \"RSU4.1\" },{ \"RSUIp\": \"192.168.00.XX\", \"SecurityLevel\":\"authPriv\", \"SNMPPort\": \"162\", \"AuthPassPhrase\": \"tester\", \"User\": \"authPrivUser\", \"RSUMIBVersion\": \"RSU4.1\" }] }", - "description":"Configurations of the RSUs the V2X hub is connected to." + "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/src/MUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp new file mode 100644 index 000000000..bc0bf53b6 --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.cpp @@ -0,0 +1,59 @@ +#include "MUSTSensorDetection.h" + +namespace MUSTSensorDriverPlugin { + + + MUSTSensorDetection csvToDectection(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.size() != 9 ){ + throw std::runtime_error("Failed to parse csv MUST Detection data"); + } + // Read out CSV information + detection.cl = fromStringToDetectionClassification(&csv.at(0)); + detection.position_x = std::stod(&csv.at(1)); + detection.position_y = std::stod(&csv.at(2)); + detection.heading = std::stod(&csv.at(3)); + detection.speed = std::stod(&csv.at(4)); + detection.size = fromStringToDetectionSize(&csv.at(5)); + detection.confidence = std::stod(&csv.at(6)); + detection.trackID = std::stoi(&csv.at(7)); + detection.timestamp = std::stoll(&csv.at(8)); + return detection; + } + + tmx::messages::simulation::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection) { + tmx::messages::simulation::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; + return detectedObject; + } + const DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept { + try { + + return stringToDetectionClassificationMap.at(str); + } + catch( const std::out_of_range &e) { + return DetectionClassification::NA; + } + } + + const DetectionSize fromStringToDetectionSize(const std::string &str) noexcept { + try { + + return stringToDetectionSizeMap.at(str); + } + catch( const std::out_of_range &e) { + return DetectionSize::NA; + } + }; +} \ No newline at end of file diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h new file mode 100644 index 000000000..c99293d44 --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDetection.h @@ -0,0 +1,58 @@ +#pragma once +#include +#include +#include // std::out_of_range +#include + +namespace MUSTSensorDriverPlugin { + + enum class DetectionClassification { + SEDAN, + TRUCK, + VAN, + NA + }; + + enum class DetectionSize + { + SMALL, + MEDIUM, + LARGE, + NA + }; + + const static std::unordered_map stringToDetectionSizeMap = { + { "small", DetectionSize::SMALL}, + { "medium", DetectionSize::MEDIUM}, + { "large", DetectionSize::LARGE} + }; + + const static std::unordered_map stringToDetectionClassificationMap = { + {"sedan", DetectionClassification::SEDAN}, + {"truck", DetectionClassification::TRUCK}, + {"van", DetectionClassification::VAN} + }; + + const DetectionSize fromStringToDetectionSize(const std::string &str) noexcept; + + const DetectionClassification fromStringToDetectionClassification(const std::string &str) noexcept; + + struct MUSTSensorDetection { + DetectionClassification cl = DetectionClassification::NA; + double position_x = 0; + double position_y = 0; + double heading = 0; + double speed = 0; + DetectionSize size = DetectionSize::NA; + double confidence = 0; + unsigned trackID = 0; + unsigned long timestamp = 0; + + }; + + + MUSTSensorDetection csvToDectection(const std::string &csv ); + + tmx::messages::simulation::SensorDetectedObject mustDectionToSensorDetectedObject(const MUSTSensorDetection &detection); + +} \ 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 bdf84dd7d..d3f2c8991 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.cpp @@ -22,7 +22,7 @@ namespace MUSTSensorDriverPlugin { MUSTSensorDriverPlugin::MUSTSensorDriverPlugin(const string &name): PluginClientClockAware(name) { - + mustSensorPacketReceiverThread = std::make_unique(std::chrono::milliseconds(5)); // Subscribe to all messages specified by the filters above. SubscribeToMessages(); } @@ -34,8 +34,37 @@ namespace MUSTSensorDriverPlugin { // 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); + std::string ip_address; + unsigned int port; + GetConfigValue("DetectionReceiverIP", ip_address); + GetConfigValue("DetectionReceiverPort", port); + createUdpServer(ip_address, port); + auto message_receiver_tick_id = mustSensorPacketReceiverThread->AddPeriodicTick([this]() { + this->processMUSTSensorDetection(); + } // end of lambda expression + , std::chrono::milliseconds(5) ); + mustSensorPacketReceiverThread->Start(); + } + } + void MUSTSensorDriverPlugin::processMUSTSensorDetection(){ + if (mustSensorPacketReceiver) { + MUSTSensorDetection detection = csvToDectection(mustSensorPacketReceiver->stringTimedReceive()); + tmx::messages::simulation::SensorDetectedObject msg = mustDectionToSensorDetectedObject(detection); + PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl; + this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); + } } + 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) { diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h index 9e8531a53..3ac68c1c8 100644 --- a/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h +++ b/src/v2i-hub/MUSTSensorDriverPlugin/src/MUSTSensorDriverPlugin.h @@ -17,6 +17,12 @@ #include #include +#include +#include +#include + +#include "MUSTSensorDetection.h" + @@ -35,13 +41,16 @@ namespace MUSTSensorDriverPlugin const char* keySensorConnectionStatus = "Sensor Connection Status"; std::unique_ptr mustSensorPacketReceiver; + + std::unique_ptr mustSensorPacketReceiverThread; /** * @brief Callback triggered on configuration updates */ void UpdateConfigSettings(); void OnConfigChanged(const char *key, const char *value) override; - void initializeMustSensorPacketReceiver(); - + void createUdpServer(const std::string &address, unsigned int port); + + void processMUSTSensorDetection(); public: /** diff --git a/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp new file mode 100644 index 000000000..3dcf34550 --- /dev/null +++ b/src/v2i-hub/MUSTSensorDriverPlugin/test/TestMUSTSensorDetection.cpp @@ -0,0 +1,22 @@ +#include +#include + +using namespace MUSTSensorDriverPlugin; + +TEST(TestMUSTSensorDetection, fromStringToDetectionSize) +{ + ASSERT_EQ(DetectionSize::SMALL, fromStringToDetectionSize("small")); + ASSERT_EQ(DetectionSize::MEDIUM, fromStringToDetectionSize("medium")); + ASSERT_EQ(DetectionSize::LARGE, fromStringToDetectionSize("large")); + ASSERT_EQ(DetectionSize::NA, fromStringToDetectionSize("not_a_size")); + +} + +TEST(TestMUSTSensorDetection, fromStringToDetectionClassification) +{ + ASSERT_EQ(DetectionClassification::SEDAN, fromStringToDetectionClassification("sedan")); + ASSERT_EQ(DetectionClassification::VAN, fromStringToDetectionClassification("van")); + ASSERT_EQ(DetectionClassification::TRUCK, fromStringToDetectionClassification("truck")); + ASSERT_EQ(DetectionClassification::NA, fromStringToDetectionClassification("not_a_classification")); + +} \ No newline at end of file