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/configuration/README.md b/configuration/README.md index d1ff943de..244f180b5 100644 --- a/configuration/README.md +++ b/configuration/README.md @@ -59,3 +59,17 @@ To support execution in a simulated environment, V2X-Hub is in the process of in * **V2XHUB_IP** – Environment variable for storing IP address of V2X Hub. * **INFRASTRUCTURE_ID** – Environment variable for storing infrastructure id of V2X Hub. * **SENSOR_JSON_FILE_PATH** – Environment variable for storing path to sensor configuration file. This is an optional simulation environment variable that allows for setting up simulated sensor for a V2X-Hub instance. Example file can be found in the **CDASimAdapterPlugin** tests [here](../src/v2i-hub/CDASimAdapter/test/sensors.json). + +### Access V2X-Hub +To access V2X-Hub UI, either chromium or google-chrome browser can be used by running the following commands: +``` +chromium-browser --ignore-certificate-errors +``` +or + +``` +google-chrome --ignore-certificate-errors + ``` + +> [!NOTE] +> V2X-Hub Server uses a secure WebSocket connection to communicate with the browser. For this, V2X-Hub Server uses a self-signed certificate instead of one signed by a trusted certificate authority (CA). Most of the browsers do not trust this until explicitly told to do so by navigating to the WebSocket URL (https://:19760) and accepting the risk. The `--ignore-certificate-errors` option instructs the browser to ignore certificate errors and warnings which removes manual effort of accepting this risk on V2X-Hub deployments. **Using the browser with this option enabled to access the internet or any other application is not advised as it is a security risk**. \ No newline at end of file diff --git a/configuration/docker-compose.yml b/configuration/docker-compose.yml index 3938dc5cc..61f61926d 100755 --- a/configuration/docker-compose.yml +++ b/configuration/docker-compose.yml @@ -40,7 +40,7 @@ services: - SIMULATION_MODE=${SIMULATION_MODE:-false} - SIMULATION_IP=${SIMULATION_IP:-127.0.0.1} - SIMULATION_REGISTRATION_PORT=6767 - - LOCAL_IP=${LOCAL_IP:-127.0.0.1} + - LOCAL_IP=${V2XHUB_IP:-127.0.0.1} - TIME_SYNC_TOPIC=time_sync - TIME_SYNC_PORT=7575 - SIM_V2X_PORT=5757 diff --git a/configuration/initialization.sh b/configuration/initialization.sh index e6dc5acf0..ba43e353a 100755 --- a/configuration/initialization.sh +++ b/configuration/initialization.sh @@ -23,9 +23,12 @@ latest_version=$(echo "$release_info" | grep -o '"tag_name": *"[^"]*"' | cut -d # Fetching all tags from Git repository tags=$(git ls-remote --tags https://github.com/usdot-fhwa-OPS/V2X-Hub.git | awk -F/ '{print $3}' | sort -V) +# Remove curly braces, Properties found, and duplicate entries +updated_tags=$(echo "$tags" | sed 's/\^{}//' | grep -v '^Properties_Found$' | awk '!seen[$0]++') + # Displaying all available versions echo "Available versions:" -echo "$tags" +echo "$updated_tags" # select a version or accept the latest version as default read -r -p "Enter V2X-Hub Version (choose from the above, or press Enter to use latest version $latest_version): " chosen_version @@ -148,5 +151,4 @@ fi cd "$mysqlDir" || return # return in case cd fails ./add_v2xhub_user.bash -chromium-browser "http://127.0.0.1" > /dev/null 2>&1 & -chromium-browser "https://127.0.0.1:19760" > /dev/null 2>&1 & +chromium-browser --ignore-certificate-errors localhost > /dev/null 2>&1 & diff --git a/configuration/mysql/pvl_lab/README.md b/configuration/mysql/pvl_lab/README.md new file mode 100644 index 000000000..f96ce81ac --- /dev/null +++ b/configuration/mysql/pvl_lab/README.md @@ -0,0 +1,11 @@ +# PVL Actions +These are Port Drayage actions created for the PVL Lab for testing of C1T functionality. + +## Instructions +Replace the port_drayage.sql file in docker-compose.yml with the file in this directory. +``` +db: + image: mysql:8.0 + volumes: + - ./mysql/pvl_lab/port_drayage.sql:/docker-entrypoint-initdb.d/port_drayage.sql +``` diff --git a/configuration/mysql/pvl_lab/port_drayage.sql b/configuration/mysql/pvl_lab/port_drayage.sql new file mode 100644 index 000000000..b1426768e --- /dev/null +++ b/configuration/mysql/pvl_lab/port_drayage.sql @@ -0,0 +1,49 @@ +-- MySQL 8.0 for Linux amd64 (x86_64) and arm64 (aarch64) +-- +-- Host: 127.0.0.1 Database: PORT_DRAYAGE +-- ------------------------------------------------------ +-- Server version 7.6.0 +-- Current Database: `PORT_DRAYAGE` +-- + +CREATE DATABASE /*!32312 IF NOT EXISTS*/ `PORT_DRAYAGE` /*!40100 DEFAULT CHARACTER SET latin1 */; + +USE `PORT_DRAYAGE`; + +-- +-- Table structure for table `first_action` +-- + +DROP TABLE IF EXISTS `first_action`; +CREATE TABLE `first_action` ( + `cmv_id` varchar(20) NOT NULL, + `cargo_id` varchar(20) DEFAULT NULL, + `destination_lat` decimal(9,7) NOT NULL, + `destination_long` decimal(9,7) NOT NULL, + `operation` varchar(20) NOT NULL, + `action_id` varchar(36) NOT NULL, + `next_action` varchar(36) NOT NULL +) ENGINE=InnoDB DEFAULT CHARSET=latin1; + +LOCK TABLES `first_action` WRITE; +INSERT INTO `first_action` VALUES ('C1T-1','CARGO_A',-5.8,3.7,'PICKUP','4bea1c45-e421-11eb-a8cc-000c29ae3c1t','32320c8a-e422-11eb-a8cc-000c29ae3c1t'); +UNLOCK TABLES; + +-- +-- Table structure for table `freight` +-- + +DROP TABLE IF EXISTS `freight`; +CREATE TABLE `freight` ( + `cmv_id` varchar(20) NOT NULL, + `cargo_id` varchar(20) DEFAULT NULL, + `destination_lat` decimal(9,7) NOT NULL, + `destination_long` decimal(9,7) NOT NULL, + `operation` varchar(20) NOT NULL, + `action_id` varchar(36) NOT NULL, + `next_action` varchar(36) NOT NULL +) ENGINE=InnoDB DEFAULT CHARSET=latin1; + +LOCK TABLES `freight` WRITE; +INSERT INTO `freight` VALUES ('C1T-1',NULL,0.2,0.0,'EXIT_STAGING_AREA','32320c8a-e422-11eb-a8cc-000c29ae3c1t','4ace39e6-ee36-11eb-9a03-0242ac130c1t'); +UNLOCK TABLES; diff --git a/docs/Release_notes.md b/docs/Release_notes.md index 48e081dee..385b8849e 100644 --- a/docs/Release_notes.md +++ b/docs/Release_notes.md @@ -1,6 +1,28 @@ V2X-Hub Release Notes --------------------------------- +Version 7.8.0, released Aug 26th, 2024 +-------------------------------------------------------- + +**Summary:** +V2X Hub release 7.8.0 includes significant enhancements such as a new telematics module for streaming V2X Hub data into the telematics server and Influx database, and an RSU Health Monitor to stream J2735 data from V2X Hub to the cloud in near real-time. The telematics plugin now subscribes to all TMX messages from V2X Hub, improving data integration. Additionally, this release addresses key issues, such as fixing the telematics bridge memory usage and correcting the handling of null topics, ensuring accurate data management and system reliability. + +Enhancement in this release: + +- V2X-Hub PR 564: Implemented a RSU Health Monitor Plugin to directly interface with RSUs connected to V2X Hub via SNMP protocol. +- V2X-Hub PR 565: Implemented a telematics plugin to subscribe to all TMX messages from V2X Hub. +- V2X-Hub PR 567: Implemented the telematic bridge to stream J2735 data from V2XHub to the cloud in near real-time. This includes forwarding JSON messages, + registering with WFD cloud services, mapping TMX message types to topics, and streaming requested TMX messages in JSON format. +- Issue 591: Created a telematics module to stream V2X Hub data into the telematics server and Influx database. This includes connecting to V2X Hub, + subscribing/unsubscribing to topics, and streaming data to the telematics tool. +- V2X-Hub PR 599: Fixed memory usage issue in the telematics bridge, which increased indefinitely when forwarding messages to the telematics tool. +- V2X-Hub PR 606: Updated the RSU Health Monitor plugin to support monitoring the health status of multiple RSUs per V2X Hub instance. +- V2X-Hub PR 613: Added configuration parameters for RSU Health Monitor Plugin to identify the source of payload when multiple RSUs are connected. + +Fixes in this release: + +- N/A + Version 7.7.0, released Aug 15th, 2024 -------------------------------------------------------- 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(); +} diff --git a/src/v2i-hub/MapPlugin/manifest.json b/src/v2i-hub/MapPlugin/manifest.json index c63f1c718..8cd561b34 100644 --- a/src/v2i-hub/MapPlugin/manifest.json +++ b/src/v2i-hub/MapPlugin/manifest.json @@ -31,7 +31,7 @@ { "key":"MAP_Files", "default":"{ \"MapFiles\": [ {\"Action\":0, \"FilePath\":\"/var/www/plugins/MAP/MAP_9709_UPER.txt\"}] }", - "description":"JSON data defining a list of map files. One map file for each action set specified by the TSC." + "description":"JSON data defining a list of map files. One map file for each action set specified by the TSC." } ] } diff --git a/src/v2i-hub/MapPlugin/src/MapPlugin.cpp b/src/v2i-hub/MapPlugin/src/MapPlugin.cpp index 2317e7800..7ee8d4ae7 100644 --- a/src/v2i-hub/MapPlugin/src/MapPlugin.cpp +++ b/src/v2i-hub/MapPlugin/src/MapPlugin.cpp @@ -1,457 +1,397 @@ +#include "MapPlugin.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include "XmlMapParser.h" -#include "ConvertToJ2735r41.h" -#include "inputs/isd/ISDToJ2735r41.h" - -#define USE_STD_CHRONO -#include -#include - -#include "utils/common.h" -#include "utils/map.h" - -#include -using namespace std; -using namespace tmx; -using namespace tmx::messages; using namespace tmx::utils; namespace MapPlugin { -#if SAEJ2735_SPEC < 63 -UPERframe _uperFrameMessage; -#endif - -class MapFile: public tmx::message { -public: - MapFile(): tmx::message() {} - virtual ~MapFile() {} - - std_attribute(this->msg, int, Action, -1, ); - std_attribute(this->msg, std::string, FilePath, "", ); - std_attribute(this->msg, std::string, InputType, "", ); - std_attribute(this->msg, std::string, Bytes, "", ); -public: - static tmx::message_tree_type to_tree(MapFile m) { - return tmx::message::to_tree(static_cast(m)); + MapPlugin::MapPlugin(const std::string &name) : PluginClientClockAware(name) { + AddMessageFilter(IVPMSG_TYPE_SIGCONT, "ACT", IvpMsgFlags_None); + SubscribeToMessages(); + errThrottle.set_Frequency(std::chrono::minutes(30)); } - static MapFile from_tree(tmx::message_tree_type tree) { - MapFile m; - m.set_contents(tree); - return m; - } -}; - -//int _mapAction = -1; -//bool _isMapFilesNew = false; -//bool _isMapLoaded = false; - -volatile int gMessageCount = 0; - -class MapPlugin: public PluginClientClockAware { -public: - MapPlugin(string name); - virtual ~MapPlugin(); - - virtual int Main(); -protected: - void UpdateConfigSettings(); - - // Virtual method overrides. - void OnConfigChanged(const char *key, const char *value); - void OnMessageReceived(IvpMessage *msg); - void OnStateChange(IvpPluginState state); - -private: - std::atomic _mapAction {-1}; - std::atomic _isMapFileNew {false}; - std::atomic _cohdaR63 {false}; - - std::map _mapFiles; - std::mutex data_lock; - - J2735MessageFactory factory; + void MapPlugin::UpdateConfigSettings() { + GetConfigValue("Frequency", sendFrequency); - int sendFrequency = 1000; - FrequencyThrottle errThrottle; + message_tree_type rawMapFiles; + GetConfigValue("MAP_Files", rawMapFiles); - bool LoadMapFiles(); - void DebugPrintMapFiles(); -}; - -MapPlugin::MapPlugin(string name) : - PluginClientClockAware(name) { - AddMessageFilter(IVPMSG_TYPE_SIGCONT, "ACT", IvpMsgFlags_None); - SubscribeToMessages(); - errThrottle.set_Frequency(std::chrono::minutes(30)); -} - -MapPlugin::~MapPlugin() { - -} + if (!rawMapFiles.empty()) { + try + { + lock_guard lock(data_lock); + _mapFiles.clear(); -void MapPlugin::UpdateConfigSettings() { - GetConfigValue("Frequency", sendFrequency); + tmx::message mapFiles; + mapFiles.set_contents(rawMapFiles); - message_tree_type rawMapFiles; - GetConfigValue("MAP_Files", rawMapFiles); + PLOG(logDEBUG) << "Got MAP_Files: " << mapFiles; - if (!rawMapFiles.empty()) { - try - { - lock_guard lock(data_lock); - _mapFiles.clear(); + for (auto mapFile : mapFiles.template get_array("MapFiles")) + { + if (mapFile.get_Action() < 0) + continue; - tmx::message mapFiles; - mapFiles.set_contents(rawMapFiles); + _mapFiles[mapFile.get_Action()] = mapFile; + _isMapFileNew = true; + } - PLOG(logDEBUG) << "Got MAP_Files: " << mapFiles; + // Check to see if the active map was lost + if (!_mapFiles.count(_mapAction)) + { + if (_mapAction > 0) + { + PLOG(logINFO) << "New configuration does not contain a map for active action " << + _mapAction << ". Using default action."; + } + _mapAction = -1; + } - for (auto mapFile : mapFiles.template get_array("MapFiles")) - { - if (mapFile.get_Action() < 0) - continue; + if (_mapFiles.size() > 0 && _mapAction < 0) + _mapAction = _mapFiles.begin()->first; - _mapFiles[mapFile.get_Action()] = mapFile; - _isMapFileNew = true; } - - // Check to see if the active map was lost - if (!_mapFiles.count(_mapAction)) + catch (exception &ex) { - if (_mapAction > 0) - { - PLOG(logINFO) << "New configuration does not contain a map for active action " << - _mapAction << ". Using default action."; - } - _mapAction = -1; + PLOG(logERROR) << "Unable to parse map file input: " << ex.what(); } - if (_mapFiles.size() > 0 && _mapAction < 0) - _mapAction = _mapFiles.begin()->first; - - } - catch (exception &ex) - { - PLOG(logERROR) << "Unable to parse map file input: " << ex.what(); + DebugPrintMapFiles(); } - DebugPrintMapFiles(); } -} - -void MapPlugin::OnConfigChanged(const char *key, const char *value) { - PluginClient::OnConfigChanged(key, value); + void MapPlugin::OnConfigChanged(const char *key, const char *value) { + PluginClient::OnConfigChanged(key, value); - if (_plugin->state == IvpPluginState_registered) - { - // Check for special case Cohda R63 messages - if (strcmp("Cohda R63", key)) + if (_plugin->state == IvpPluginState_registered) { - string strValue(value); - - if (boost::iequals(strValue, "1") - || boost::iequals(strValue, "true") - || boost::iequals(strValue, "t") - || boost::iequals(strValue, "on")) + // Check for special case Cohda R63 messages + if (strcmp("Cohda R63", key)) { - _cohdaR63 = true; + std::string strValue(value); + + if (boost::iequals(strValue, "1") + || boost::iequals(strValue, "true") + || boost::iequals(strValue, "t") + || boost::iequals(strValue, "on")) + { + _cohdaR63 = true; + } + else + { + _cohdaR63 = false; + } } else { - _cohdaR63 = false; + UpdateConfigSettings(); } } - else - { - UpdateConfigSettings(); - } } -} -void MapPlugin::OnStateChange(IvpPluginState state) { - PluginClientClockAware::OnStateChange(state); + void MapPlugin::OnStateChange(IvpPluginState state) { + PluginClientClockAware::OnStateChange(state); - if (state == IvpPluginState_registered) { - UpdateConfigSettings(); + if (state == IvpPluginState_registered) { + UpdateConfigSettings(); + } } -} -void MapPlugin::OnMessageReceived(IvpMessage *msg) { - PluginClient::OnMessageReceived(msg); + void MapPlugin::OnMessageReceived(IvpMessage *msg) { + PluginClient::OnMessageReceived(msg); - if ((strcmp(msg->type, IVPMSG_TYPE_SIGCONT) == 0) - && (strcmp(msg->subtype, "ACT") == 0) - && (msg->payload->type == cJSON_String)) { - int action = ivpSigCont_getIvpSignalControllerAction(msg); + if ((strcmp(msg->type, IVPMSG_TYPE_SIGCONT) == 0) + && (strcmp(msg->subtype, "ACT") == 0) + && (msg->payload->type == cJSON_String)) { + int action = ivpSigCont_getIvpSignalControllerAction(msg); - if (action != _mapAction) - { - // Ignore if there is no map for this action - lock_guard lock(data_lock); - if (_mapFiles.count(action) <= 0) + if (action != _mapAction) { - if (errThrottle.Monitor(action)) + // Ignore if there is no map for this action + lock_guard lock(data_lock); + if (_mapFiles.count(action) <= 0) { - PLOG(logERROR) << "Missing map for Action " << action; + if (errThrottle.Monitor(action)) + { + PLOG(logERROR) << "Missing map for Action " << action; + } + return; } - return; - } - _isMapFileNew = _mapAction.exchange(action) != action; + _isMapFileNew = _mapAction.exchange(action) != action; + } } } -} -int MapPlugin::Main() { - PLOG(logINFO) << "Starting plugin."; + int MapPlugin::Main() { + PLOG(logINFO) << "Starting plugin."; - bool mapFilesOk = false; + bool mapFilesOk = false; - std::unique_ptr msg; - int activeAction = -1; - - // wait for the clock to be initialized - getClock()->wait_for_initialization(); + std::unique_ptr msg; + int activeAction = -1; + + // wait for the clock to be initialized + getClock()->wait_for_initialization(); - while (_plugin->state != IvpPluginState_error) { - if (_isMapFileNew) { - msg.reset(); - activeAction = -1; + while (_plugin->state != IvpPluginState_error) { + if (_isMapFileNew) { + msg.reset(); + activeAction = -1; - mapFilesOk = LoadMapFiles(); - _isMapFileNew = false; - } + mapFilesOk = LoadMapFiles(); + _isMapFileNew = false; + } - int temp = _mapAction; - if (temp < 0) - { - // No action set yet, so just wait - sleep(1); - continue; - } + int temp = _mapAction; + if (temp < 0) + { + // No action set yet, so just wait + sleep(1); + continue; + } - // Action has changed, so retrieve the correct map - if (temp != activeAction) - { - lock_guard lock(data_lock); - string byteStr = _mapFiles[temp].get_Bytes(); - if (!byteStr.empty()) + // Action has changed, so retrieve the correct map + if (temp != activeAction) { - msg.reset(dynamic_cast(factory.NewMessage(api::MSGSUBTYPE_MAPDATA_STRING))); - if (!msg) - { if (errThrottle.Monitor(temp)) - { - PLOG(logERROR) << "Unable to create map from bytes " << byteStr << ": " << factory.get_event(); + lock_guard lock(data_lock); + std::string byteStr = _mapFiles[temp].get_Bytes(); + if (!byteStr.empty()) + { + msg.reset(dynamic_cast(factory.NewMessage(tmx::messages::api::MSGSUBTYPE_MAPDATA_STRING))); + if (!msg) + { if (errThrottle.Monitor(temp)) + { + PLOG(logERROR) << "Unable to create map from bytes " << byteStr << ": " << factory.get_event(); + } + sleep(1); + continue; } - sleep(1); - continue; - } - string enc = msg->get_encoding(); - msg->refresh_timestamp(); - msg->set_payload(byteStr); - msg->set_encoding(enc); - msg->set_flags(IvpMsgFlags_RouteDSRC); - msg->addDsrcMetadata(0x8002); + std::string enc = msg->get_encoding(); + msg->refresh_timestamp(); + msg->set_payload(byteStr); + msg->set_encoding(enc); + msg->set_flags(IvpMsgFlags_RouteDSRC); + msg->addDsrcMetadata(tmx::messages::api::mapData_PSID); - activeAction = temp; - PLOG(logINFO) << "Map for action " << activeAction << " will be sent"; + activeAction = temp; + PLOG(logINFO) << "Map for action " << activeAction << " will be sent"; + } } - } - if (mapFilesOk) - { - // Time to send a new message - routeable_message *rMsg = dynamic_cast(msg.get()); - if (_cohdaR63) + if (mapFilesOk) { - auto bytes = rMsg->get_payload_bytes(); - rMsg->set_payload_bytes(bytes); // TODO: Translate to R63 bytes - } + // Time to send a new message + routeable_message *rMsg = dynamic_cast(msg.get()); + if (_cohdaR63) + { + auto bytes = rMsg->get_payload_bytes(); + rMsg->set_payload_bytes(bytes); // TODO: Translate to R63 bytes + } - if (rMsg) { - rMsg->refresh_timestamp(); - BroadcastMessage(*rMsg); + if (rMsg) { + rMsg->refresh_timestamp(); + BroadcastMessage(*rMsg); + } } + + auto sleepUntil = getClock()->nowInMilliseconds() + sendFrequency; + getClock()->sleep_until(sleepUntil); } - auto sleepUntil = getClock()->nowInMilliseconds() + sendFrequency; - getClock()->sleep_until(sleepUntil); + return (EXIT_SUCCESS); } - return (EXIT_SUCCESS); -} + std::string MapPlugin::enum_to_hex_string() + { + std::snprintf(mapID_buffer.data(), mapID_buffer.size(), "%04X", tmx::messages::api::mapData); + std::string map_messageID(mapID_buffer.data()); -bool MapPlugin::LoadMapFiles() -{ - if (_mapFiles.empty()) - return false; + return map_messageID; + } - lock_guard lock(data_lock); - for (auto &mapPair : _mapFiles) + std::string MapPlugin::removeMessageFrame(const std::string &fileContent) { - MapFile &mapFile = mapPair.second; - if (mapFile.get_Bytes() == "") - { - // Fill in the bytes for each map file - string inType = mapFile.get_InputType(); - if (inType.empty()) - { - try - { - string fn = mapFile.get_FilePath(); - - if (fn.substr(fn.size() - 5) == ".json") - inType = "ISD"; - else if (fn.substr(fn.size() - 4) == ".txt") - inType = "TXT"; - else if (fn.substr(fn.size()- 5) == ".uper") - inType ="UPER"; - else - inType = "XML"; - - if (inType == "ISD") - { - ISDToJ2735r41 converter(fn); - mapFile.set_Bytes(converter.to_encoded_message().get_payload_str()); - - PLOG(logINFO) << fn << " ISD file encoded as " << mapFile.get_Bytes(); - } - else if (inType == "TXT") - { - byte_stream bytes; - ifstream in(fn); - in >> bytes; + std::string map_messageID = enum_to_hex_string(); - PLOG(logINFO) << fn << " MAP encoded bytes are " << bytes; + // Check for and remove MessageFrame + if (fileContent.size() >= 4 && fileContent.substr(0, 4) == map_messageID) + { + // Check if message is hex size > 255, remove appropriate header + std::string tempFrame = fileContent; + std::string newFrame = fileContent; + tempFrame.erase(0, 6); + PLOG(logDEBUG4) << "Checking size of: " << tempFrame; + auto headerSize = (tempFrame.size() > 510) ? 8 : 6; + newFrame.erase(0, headerSize); + + PLOG(logDEBUG4) << "Payload without MessageFrame: " << newFrame; + return newFrame; + } + else + { + return fileContent; + } + } - MapDataMessage *mapMsg = MapDataEncodedMessage::decode_j2735_message >(bytes); - if (mapMsg) { - PLOG(logDEBUG) << "Map is " << *mapMsg; + std::string MapPlugin::checkMapContent(const std::string &fn) + { + PLOG(logDEBUG4) << "In MapPlugin :: checkMapContent"; + try + { + std::ifstream in(fn.c_str(), std::ios::binary); + if (!in) + { + PLOG(logERROR) << "Failed to open file: " << fn.c_str(); + throw std::ios_base::failure("Failed to open file: " + fn); + } + else + { + std::string content((std::istreambuf_iterator(in)), std::istreambuf_iterator()); + in.close(); + // Remove any newline characters + content.erase(remove(content.begin(), content.end(), '\n'), content.end()); + PLOG(logDEBUG4) << "Map without newline " << content; + std::string payload = removeMessageFrame(content); + + return payload; + } + } + catch (const std::ios_base::failure& e) + { + PLOG(logERROR) << "Exception encountered while reading file: " << e.what(); + throw; + } + } - MapDataEncodedMessage mapEnc; - mapEnc.encode_j2735_message(*mapMsg); - mapFile.set_Bytes(mapEnc.get_payload_str()); + bool MapPlugin::LoadMapFiles() + { + if (_mapFiles.empty()) + return false; - PLOG(logINFO) << fn << " J2735 message bytes encoded as " << mapFile.get_Bytes(); - } - } - else if (inType == "UPER") - { - PLOG(logDEBUG) << "Reading MAP file as UPER encoded hex bytes including MessageFrame." << std::endl; - std::ifstream in; - try { - in.open(fn, std::ios::in | std::ios::binary ); - if (in.is_open()) { - in.seekg(0, std::ios::end); - int fileSize = in.tellg(); - in.seekg(0, std::ios::beg); - PLOG(logDEBUG) << "File size is " << fileSize <(in)), std::istreambuf_iterator()); - PLOG(logDEBUG) << "File contents : " << bytes_string << std::endl; - mapFile.set_Bytes(bytes_string); - } - else { - PLOG(logERROR) << "Failed to open file " << fn << "." << std::endl; - } - } - catch( const ios_base::failure &e) { - PLOG(logERROR) << "Exception Encountered : \n" << e.what(); - } - } - else if (inType == "XML") + lock_guard lock(data_lock); + for (auto &mapPair : _mapFiles) + { + MapFile &mapFile = mapPair.second; + if (mapFile.get_Bytes() == "") + { + // Fill in the bytes for each map file + std::string inType = mapFile.get_InputType(); + if (inType.empty()) + { + try { - tmx::message_container_type container; - container.load(fn); + std::string fn = mapFile.get_FilePath(); + + if (fn.substr(fn.size() - 5) == ".json") + inType = "ISD"; + else if (fn.substr(fn.size() - 4) == ".txt") + inType = "TXT"; + else if (fn.substr(fn.size() - 4) == ".xml") + inType = "XML"; + else + PLOG(logWARNING) << "Incorrect MapFile extension entered!"; + + if (inType == "ISD") + { + ISDToJ2735r41 converter(fn); + mapFile.set_Bytes(converter.to_encoded_message().get_payload_str()); - if (container.get_storage().get_tree().begin()->first == "MapData") + PLOG(logINFO) << fn << " ISD file encoded as " << mapFile.get_Bytes(); + } + else if (inType == "TXT") { - MapDataMessage mapMsg; - mapMsg.set_contents(container.get_storage().get_tree()); + std::string payload = checkMapContent(fn); + byte_stream bytes; + std::istringstream streamableContent(payload); + streamableContent >> bytes; + PLOG(logINFO) << "MAP encoded bytes are " << bytes; + tmx::messages::MapDataMessage *mapMsg = tmx::messages::MapDataEncodedMessage::decode_j2735_message>(bytes); + + if (mapMsg) + { + PLOG(logDEBUG) << "Map is " << *mapMsg; - PLOG(logDEBUG) << "Encoding " << mapMsg; - MapDataEncodedMessage mapEnc; - mapEnc.encode_j2735_message(mapMsg); - mapFile.set_Bytes(mapEnc.get_payload_str()); + tmx::messages::MapDataEncodedMessage mapEnc; + mapEnc.encode_j2735_message(*mapMsg); + mapFile.set_Bytes(mapEnc.get_payload_str()); - PLOG(logINFO) << fn << " XML file encoded as " << mapFile.get_Bytes(); + PLOG(logINFO) << "J2735 message bytes encoded as " << mapFile.get_Bytes(); + } } - else + else if (inType == "XML") { - ConvertToJ2735r41 mapConverter; - XmlMapParser mapParser; - map theMap; + tmx::message_container_type container; + container.load(fn); - if (mapParser.ReadGidFile(fn, &theMap)) + if (container.get_storage().get_tree().begin()->first == "MapData") { - mapConverter.convertMap(&theMap); + tmx::messages::MapDataMessage mapMsg; + mapMsg.set_contents(container.get_storage().get_tree()); - PLOG(logDEBUG) << "Encoded Bytes:" << mapConverter.encodedByteCount; + PLOG(logDEBUG) << "Encoding " << mapMsg; + tmx::messages::MapDataEncodedMessage mapEnc; + mapEnc.encode_j2735_message(mapMsg); + mapFile.set_Bytes(mapEnc.get_payload_str()); - if (mapConverter.encodedByteCount > 0) + PLOG(logINFO) << fn << " XML file encoded as: " << mapFile.get_Bytes(); + } + else + { + ConvertToJ2735r41 mapConverter; + XmlMapParser mapParser; + map theMap; + + if (mapParser.ReadGidFile(fn, &theMap)) { - byte_stream bytes(mapConverter.encodedByteCount); - memcpy(bytes.data(), mapConverter.encoded, mapConverter.encodedByteCount); + mapConverter.convertMap(&theMap); - auto *mapEnc = factory.NewMessage(bytes); - if (!mapEnc) - return false; + PLOG(logDEBUG) << "Encoded Bytes:" << mapConverter.encodedByteCount; - mapFile.set_Bytes(mapEnc->get_payload_str()); + if (mapConverter.encodedByteCount > 0) + { + byte_stream bytes(mapConverter.encodedByteCount); + memcpy(bytes.data(), mapConverter.encoded, mapConverter.encodedByteCount); - PLOG(logINFO) << fn << " input file encoded as " << mapEnc->get_payload_str(); - } - else - { - return false; + auto *mapEnc = factory.NewMessage(bytes); + if (!mapEnc) + return false; + + mapFile.set_Bytes(mapEnc->get_payload_str()); + + PLOG(logINFO) << fn << " input file encoded as: " << mapEnc->get_payload_str(); + } + else + { + return false; + } } } } } - } - catch (exception &ex) - { - PLOG(logERROR) << "Unable to convert " << mapFile.get_FilePath() << ": " << ex.what(); - return false; + catch (exception &ex) + { + PLOG(logERROR) << "Unable to convert " << mapFile.get_FilePath() << ": " << ex.what(); + return false; + } } } } - } - return true; -} + return true; + } -void MapPlugin::DebugPrintMapFiles() { - PLOG(logDEBUG) << _mapFiles.size() - << " map files specified by configuration settings:"; + void MapPlugin::DebugPrintMapFiles() { + PLOG(logDEBUG) << _mapFiles.size() + << " map files specified by configuration settings:"; - for (auto iter = _mapFiles.begin(); iter != _mapFiles.end(); iter++) { - int key = iter->first; - PLOG(logDEBUG) << "-- Action " << key << " file is " << iter->second.get_FilePath(); + for (auto iter = _mapFiles.begin(); iter != _mapFiles.end(); iter++) { + int key = iter->first; + PLOG(logDEBUG) << "-- Action " << key << " file is " << iter->second.get_FilePath(); + } } -} } /* End namespace MapPlugin */ diff --git a/src/v2i-hub/MapPlugin/src/MapPlugin.h b/src/v2i-hub/MapPlugin/src/MapPlugin.h new file mode 100644 index 000000000..f0da14270 --- /dev/null +++ b/src/v2i-hub/MapPlugin/src/MapPlugin.h @@ -0,0 +1,114 @@ +//============================================================================ +// Name : MapPlugin.cpp +// Author : FHWA Saxton Transportation Operations Laboratory +// Version : 7.6.0 +// Copyright : Copyright (c) 2024 FHWA Saxton Transportation Operations Laboratory. All rights reserved. +// Description : MAP Plugin +//============================================================================ + +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include "XmlMapParser.h" +#include "ConvertToJ2735r41.h" +#include "inputs/isd/ISDToJ2735r41.h" + +#define USE_STD_CHRONO +#include +#include + +#include "utils/common.h" +#include "utils/map.h" + +#include + +namespace MapPlugin { + +#if SAEJ2735_SPEC < 63 +UPERframe _uperFrameMessage; +#endif + +class MapFile: public tmx::message { +public: + using tmx::message::message; + ~MapFile() override = default; + + MapFile(MapFile&&) noexcept = default; + MapFile& operator=(MapFile&&) noexcept = default; + + MapFile(const MapFile&) = default; + MapFile& operator=(const MapFile&) = default; + + std_attribute(this->msg, int, Action, -1, ); + std_attribute(this->msg, std::string, FilePath, "", ); + std_attribute(this->msg, std::string, InputType, "", ); + std_attribute(this->msg, std::string, Bytes, "", ); + + static tmx::message_tree_type to_tree(const MapFile &m) { + return tmx::message::to_tree(static_cast(m)); + } + + static MapFile from_tree(const tmx::message_tree_type &tree) { + MapFile m; + m.set_contents(tree); + return m; + } +}; + +class MapPlugin: public tmx::utils::PluginClientClockAware { +public: + explicit MapPlugin(const std::string &name); + int Main() override; + +protected: + void UpdateConfigSettings(); + + // Virtual method overrides. + void OnConfigChanged(const char *key, const char *value) override; + void OnMessageReceived(IvpMessage *msg) override; + void OnStateChange(IvpPluginState state) override; + + bool LoadMapFiles(); + void DebugPrintMapFiles(); + + std::string enum_to_hex_string(); + std::string removeMessageFrame(const std::string &fileContent); + std::string checkMapContent(const std::string &fn); + +private: + tmx::messages::J2735MessageFactory factory; + tmx::utils::FrequencyThrottle errThrottle; + + std::atomic _mapAction {-1}; + std::atomic _isMapFileNew {false}; + std::atomic _cohdaR63 {false}; + + std::mutex data_lock; + std::map _mapFiles; + int sendFrequency = 1000; + + std::array mapID_buffer; +}; + +} // namespace MapPlugin