diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index 649f451e2..cc113ce62 100755 --- a/.devcontainer/docker-compose-vscode.yml +++ b/.devcontainer/docker-compose-vscode.yml @@ -22,6 +22,7 @@ services: - TIME_SYNC_TOPIC=time_sync - TIME_SYNC_PORT=7575 - SIM_V2X_PORT=5757 + - SIM_INTERACTION_PORT=7576 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 - SENSOR_JSON_FILE_PATH=/var/www/plugins/MAP/sensors.json diff --git a/src/tmx/Messages/include/MessageTypes.h b/src/tmx/Messages/include/MessageTypes.h index a4a84f6f4..e0fd09c70 100644 --- a/src/tmx/Messages/include/MessageTypes.h +++ b/src/tmx/Messages/include/MessageTypes.h @@ -91,6 +91,7 @@ static CONSTEXPR const char *MSGSUBTYPE_INCOMING_STRING = "Incoming"; static CONSTEXPR const char *MSGSUBTYPE_OUTGOING_STRING = "Outgoing"; static CONSTEXPR const char *MSGSUBTYPE_SHUTDOWN_STRING = "Shutdown"; static CONSTEXPR const char *MSGSUBTYPE_TIMESYNC_STRING = "TimeSync"; +static CONSTEXPR const char *MSGSUBTYPE_SENSOR_DETECTED_OBJECT_STRING = "SensorDetectedObject"; } /* End namespace messages */ diff --git a/src/tmx/Messages/include/simulation/SensorDetectedObject.h b/src/tmx/Messages/include/simulation/SensorDetectedObject.h new file mode 100644 index 000000000..179f48a13 --- /dev/null +++ b/src/tmx/Messages/include/simulation/SensorDetectedObject.h @@ -0,0 +1,35 @@ +#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/simulation/SimulationEnvUtils.h b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h index 7b0c4ec71..7498a7922 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h @@ -46,6 +46,11 @@ namespace tmx::utils::sim{ * necessary in SIMULATION MODE for CDASim message forwarding. */ constexpr inline static const char *SIM_V2X_PORT = "SIM_V2X_PORT"; + /** + * @brief Name of environment variable for storing port for forwarding v2x messages to CDASim. Only + * necessary in SIMULATION MODE for CDASim message forwarding. + */ + constexpr inline static const char *SIM_INTERACTION_PORT= "SIM_INTERACTION_PORT"; /** * @brief Name of environment variable for storing port for receiving v2x messages from CDASim. Only * necessary in SIMULATION MODE for CDASim message forwarding. diff --git a/src/v2i-hub/CARMAStreetsPlugin/manifest.json b/src/v2i-hub/CARMAStreetsPlugin/manifest.json index 790ef1af9..f0daaf9a9 100644 --- a/src/v2i-hub/CARMAStreetsPlugin/manifest.json +++ b/src/v2i-hub/CARMAStreetsPlugin/manifest.json @@ -34,6 +34,11 @@ "type":"J2735", "subtype":"SPAT-P", "description":"Signal Phase and Timing (SPAT) status for the signalized intersection." + }, + { + "type":"Application", + "subtype":"SensorDetectedObject", + "description": "Sensor detected object for cooperative perception." } ], "configuration": [ @@ -116,6 +121,11 @@ "key": "SpatConsumerGroupId", "default": "v2xhub_spat", "description": "Apache Kafka consumer group ID for spat consumer." + }, + { + "key": "SimSensorDetectedObjTopic", + "default": "v2xhub_sim_sensor_detected_object", + "description": "Apache Kafka topic plugin will transmit simulated sensor detected object to." } ] diff --git a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index d76260ad6..c5bb9a7e6 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,6 +25,8 @@ 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 ); + SubscribeToMessages(); } @@ -48,7 +50,8 @@ void CARMAStreetsPlugin::UpdateConfigSettings() { GetConfigValue("MobilityOperationTopic", _transmitMobilityOperationTopic); GetConfigValue("MobilityPathTopic", _transmitMobilityPathTopic); GetConfigValue("MapTopic", _transmitMAPTopic); - GetConfigValue("SRMTopic", _transmitSRMTopic); + GetConfigValue("SRMTopic", _transmitSRMTopic); + GetConfigValue("SimSensorDetectedObjTopic", _transmitSimSensorDetectedObjTopic); // Populate strategies config string config; GetConfigValue("MobilityOperationStrategies", config); @@ -626,6 +629,13 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } } + +void CARMAStreetsPlugin::HandleSimulatedSensorDetectedMessage(simulation::SensorDetectedObject &msg, routeable_message &routeableMsg) +{ + PLOG(logDEBUG) << "Produce sensor detected message in JSON format: " << msg.to_string() < #include #include "JsonToJ2735SSMConverter.h" +#include #include "PluginClientClockAware.h" @@ -48,6 +49,12 @@ class CARMAStreetsPlugin: public PluginClientClockAware { void HandleMobilityOperationMessage(tsm3Message &msg, routeable_message &routeableMsg); void HandleMobilityPathMessage(tsm2Message &msg, routeable_message &routeableMsg); void HandleBasicSafetyMessage(BsmMessage &msg, routeable_message &routeableMsg); + /** + * @brief Callback function when the plugin received detected object, and forward the detected object to Kafka topic. + * @param msg Detected object received from TMX bus. + * @param routeableMsg routeable_message for detected object. + */ + void HandleSimulatedSensorDetectedMessage(simulation::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. @@ -104,6 +111,7 @@ class CARMAStreetsPlugin: public PluginClientClockAware { std::string _transmitBSMTopic; std::string _transmitMAPTopic; std::string _transmitSRMTopic; + std::string _transmitSimSensorDetectedObjTopic; std::string _kafkaBrokerIp; std::string _kafkaBrokerPort; std::shared_ptr _kafka_producer_ptr; diff --git a/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py new file mode 100755 index 000000000..e90eaf9c3 --- /dev/null +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_detected_object_udp.py @@ -0,0 +1,81 @@ + +import socket +import sys +import json +import time + +# Script for integration testing CDASimAdapter Time Sync functionality. +# This python script sends periodic time sync messages to a configurable +# host and port. To Test the Time Sync functionality of the CDASimAdapter +# set port to the value of the TIME_SYNC_PORT environment variable. +# +# TODO Move this script into a more permanent location +count_num = 0 + +def generate_sim_external_object(): + jsonResult = { + "metadata": { + "type": "Application", + "subtype": "SensorDetectedObject", + "timestamp": 123, + "isSimulated": True + }, + "payload": { + "sensor_id": "sensor1", + "proj_string": "asdlasdkasd", + "type": "Car", + "confidence": "0.7", + "objectId": "Object1", + "position": { + "x": 1.0, + "y": 2.5, + "z": 1.1 + }, + "positionCovariance" : [12,12,2, 34, 34, 55], + "velocity": { + "x": 1.0, + "y": 2.5, + "z": 1.1 + }, + "velocityCovariance" : ["a11", "a12", "a13", "a21", "a22", "a23", "a31", "a32", "a33"], + "angularVelocity":{ + "x": 1.0, + "y": 2.5, + "z": 1.1 + }, + "angularVelocityCovariance" : ["a11", "a12", "a13", "a21", "a22", "a23", "a31", "a32", "a33"], + "size": { + "length": 0.1, + "width": 0.4, + "height": 1.5 + } + } + } + jsonResult = json.dumps(jsonResult) + return jsonResult +port = 7576 +address = "127.0.0.1" +host = (address, port) +try: + sock = socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM) +except socket.error as err: + print('Socket error because of %s' %(err)) + + +while True : + try: + msg = generate_sim_external_object() + encoded_msg = str.encode(msg) + count_num += 1 + sock.sendto(encoded_msg,host) + print( encoded_msg.decode(encoding= 'UTF-8'), 'was sent to ', host) + print(f'Message sent at ${time.time()}') + time.sleep(5) + except socket.gaierror: + + print ('There an error resolving the host') + break + +sock.close() + + diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp index 297e44bdd..c662e6e9c 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -60,6 +60,7 @@ namespace CDASimAdapter{ if ( connection->is_connected() ) { start_time_sync_thread_timer(); + start_sensor_detected_object_detection_thread(); start_immediate_forward_thread(); start_message_receiver_thread(); }else { @@ -78,6 +79,11 @@ namespace CDASimAdapter{ } + void CDASimAdapter::forward_simulated_detected_message(tmx::messages::simulation::SensorDetectedObject &msg) { + PLOG(logDEBUG1) << "Sending Simulated SensorDetectedObject Message " << msg << std::endl; + this->BroadcastMessage(msg, _name, 0 , IvpMsgFlags_None); + } + bool CDASimAdapter::connect() { try { std::string simulation_ip = sim::get_sim_config(sim::SIMULATION_IP); @@ -85,6 +91,7 @@ namespace CDASimAdapter{ PLOG(logINFO) << "Simulation and local IP successfully initialized!"<< std::endl; uint simulation_registration_port = std::stoul(sim::get_sim_config(sim::SIMULATION_REGISTRATION_PORT)); uint time_sync_port = std::stoul(sim::get_sim_config(sim::TIME_SYNC_PORT)); + auto simulated_interaction_port =static_cast(std::stoi(sim::get_sim_config(sim::SIM_INTERACTION_PORT))); uint v2x_port = std::stoul(sim::get_sim_config(sim::V2X_PORT)); uint sim_v2x_port = std::stoul(sim::get_sim_config(sim::SIM_V2X_PORT)); std::string infrastructure_id = sim::get_sim_config(sim::INFRASTRUCTURE_ID); @@ -95,11 +102,11 @@ namespace CDASimAdapter{ " Time Sync Port: " << std::to_string( time_sync_port) << " and V2X Port: " << std::to_string(v2x_port) << std::endl; if ( connection ) { connection.reset(new CDASimConnection( simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, v2x_port, location, sensor_json_file_path)); + time_sync_port, simulated_interaction_port, v2x_port, location, sensor_json_file_path )); } else { connection = std::make_unique(simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, v2x_port, location, sensor_json_file_path); + time_sync_port, simulated_interaction_port, v2x_port, location, sensor_json_file_path); } } catch (const TmxException &e) { @@ -161,6 +168,34 @@ namespace CDASimAdapter{ } } + void CDASimAdapter::start_sensor_detected_object_detection_thread() { + PLOG(logDEBUG) << "Creating Thread Timer for simulated external object" << std::endl; + try + { + if(!external_object_detection_thread_timer) + { + external_object_detection_thread_timer = std::make_unique(); + } + external_object_detection_thread_timer->AddPeriodicTick([this](){ + PLOG(logDEBUG1) << "Listening for Sensor Detected Message from CDASim." << std::endl; + auto msg = connection->consume_sensor_detected_object_message(); + if ( !msg.is_empty()) { + this->forward_simulated_detected_message(msg); + } + else + { + PLOG(logDEBUG1) << "CDASim connection has not yet received an simulated sensor detected message!" << std::endl; + } + }//End lambda + , std::chrono::milliseconds(100)); + external_object_detection_thread_timer->Start(); + } + catch ( const UdpServerRuntimeError &e ) + { + PLOG(logERROR) << "Error occured :" << e.what() << std::endl; + } + } + void CDASimAdapter::attempt_message_from_v2xhub() const { try { std::string msg = connection->consume_v2x_message_from_v2xhub(); diff --git a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp index 15621be80..982ac5f64 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -5,11 +5,11 @@ using namespace tmx::utils; namespace CDASimAdapter{ CDASimConnection::CDASimConnection(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, const uint sim_v2x_port, - const std::string &local_ip, const uint time_sync_port, const uint v2x_port, + const std::string &local_ip, const uint time_sync_port,const uint simulated_interaction_port, const uint v2x_port, const Point &location, const std::string &sensor_json_file_path) : _simulation_ip(simulation_ip), _infrastructure_id(infrastructure_id), _simulation_registration_port(simulation_registration_port), - _simulation_v2x_port(sim_v2x_port), _local_ip(local_ip), _time_sync_port(time_sync_port), _v2x_port(v2x_port), - _location(location) ,_sensor_json_file_path(sensor_json_file_path) { + _simulation_v2x_port(sim_v2x_port), _local_ip(local_ip), _time_sync_port(time_sync_port), _simulated_interaction_port(simulated_interaction_port),_v2x_port(v2x_port), + _location(location) ,_sensor_json_file_path(sensor_json_file_path) { PLOG(logDEBUG) << "CARMA-Simulation connection initialized." << std::endl; } @@ -20,11 +20,11 @@ namespace CDASimAdapter{ } bool CDASimConnection::connect() { - if (!carma_simulation_handshake(_simulation_ip, _infrastructure_id, _simulation_registration_port, _local_ip, _time_sync_port, _v2x_port, _location)) { + if (!carma_simulation_handshake(_simulation_ip, _infrastructure_id, _simulation_registration_port, _local_ip, _time_sync_port, _simulated_interaction_port, _v2x_port, _location)) { _connected = false; return _connected; } - if (!setup_udp_connection(_simulation_ip, _local_ip, _time_sync_port, _v2x_port, _simulation_v2x_port )) { + if (!setup_udp_connection(_simulation_ip, _local_ip, _time_sync_port,_simulated_interaction_port, _v2x_port, _simulation_v2x_port )) { _connected = false; return _connected; } @@ -33,8 +33,9 @@ namespace CDASimAdapter{ return _connected; } - std::string CDASimConnection::get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, - const Point &location) const + + std::string CDASimConnection::get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, + const tmx::utils::Point &location) const { Json::Value message; @@ -44,6 +45,7 @@ namespace CDASimAdapter{ message["infrastructureId"] = infrastructure_id; message["rxMessagePort"] = v2x_port; message["timeSyncPort"] = time_sync_port; + message["simulatedInteractionPort"] = simulated_interaction_port; message["location"]["x"] = location.X; message["location"]["y"] = location.Y; message["location"]["z"] = location.Z; @@ -62,13 +64,13 @@ namespace CDASimAdapter{ } bool CDASimConnection::carma_simulation_handshake(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, - const std::string &local_ip, const uint time_sync_port, const uint v2x_port, - const Point &location) + const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, + const tmx::utils::Point &location) { // Create JSON message with the content std::string payload = ""; - payload = get_handshake_json(infrastructure_id, local_ip, time_sync_port, v2x_port, location); + payload = get_handshake_json(infrastructure_id, local_ip, time_sync_port, simulated_interaction_port, v2x_port, location); try { @@ -84,7 +86,7 @@ namespace CDASimAdapter{ return true; } - bool CDASimConnection::setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, + bool CDASimConnection::setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, const uint simulation_v2x_port) { try { // Iniitialize CARMA Simulation UDP Server and Client to foward V2X messages between CARMA simulation @@ -101,12 +103,14 @@ namespace CDASimAdapter{ // TODO: Replace 0 with message receiver port message_receiver_publisher = std::make_shared( local_ip, 8765); // Initialize UDP Server for listening for incoming CARMA-Simulation time synchronization. - PLOG(logDEBUG) << "Creating UDPServer Time Sync Messages" << local_ip << ":" << std::to_string(time_sync_port) << "\n" - << "Creating UDPServer for CDA V2X message forwarding" << local_ip << ":" << std::to_string(v2x_port) << "\n" - << "Creating UDPClient for CDA V2X message forwarding " << simulation_ip << ":" << std::to_string(simulation_v2x_port) << "\n" + PLOG(logDEBUG) << "Creating UDPServer for Time Sync Messages: " << local_ip << ":" << std::to_string(time_sync_port) << "\n" + << "Creating UDPServer for Simulated External Object detection: " << local_ip << ":" << std::to_string(simulated_interaction_port) << "\n" + << "Creating UDPServer for CDA V2X message forwarding: " << local_ip << ":" << std::to_string(v2x_port) << "\n" + << "Creating UDPClient for CDA V2X message forwarding: " << simulation_ip << ":" << std::to_string(simulation_v2x_port) << "\n" << "Creating UDPServer for Immediate Forward " << local_ip << ":" << std::to_string(5678) << "\n" << "Creating UDPClient for Message Receiver " << local_ip << ":" << std::to_string(8765) << std::endl; time_sync_listener = std::make_shared(local_ip,time_sync_port); + sensor_detected_object_listener = std::make_shared (local_ip, simulated_interaction_port); } catch (const UdpClientRuntimeError &e) { PLOG(logERROR) << "Encountered UDPClient Runtime error during UDP connection initialization : " << e.what() << std::endl; @@ -128,12 +132,28 @@ namespace CDASimAdapter{ msg.set_contents( str_msg ); } else { - throw std::runtime_error("Time Sync UDP Server is not initialized"); + throw UdpServerRuntimeError("Time Sync UDP Server is not initialized"); } return msg; } + tmx::messages::simulation::SensorDetectedObject CDASimConnection::consume_sensor_detected_object_message() const + { + tmx::messages::simulation::SensorDetectedObject externalObj; + externalObj.clear(); + if(sensor_detected_object_listener) + { + std::string str_msg = consume_server_message(sensor_detected_object_listener); + externalObj.set_contents(str_msg); + } + else + { + throw UdpServerRuntimeError("Simulated External Object UDP Server is not initialized."); + } + return externalObj; + } + std::string CDASimConnection::consume_hex_server_message( const std::shared_ptr _server) const { std::vector msg(4000); int num_of_bytes = _server->TimedReceive(msg.data(),4000, 5); @@ -180,7 +200,7 @@ namespace CDASimAdapter{ return msg; } else { - throw std::runtime_error("CARMA Simulation UDP Server is not initialized!"); + throw UdpServerRuntimeError("CARMA Simulation UDP Server is not initialized!"); } return ""; @@ -192,7 +212,7 @@ namespace CDASimAdapter{ return msg; } else { - throw std::runtime_error("Immediate Forward UDP Server is not initialized!"); + throw UdpServerRuntimeError("Immediate Forward UDP Server is not initialized!"); } return ""; diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp index b3a979373..529cd9dc9 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -89,6 +89,11 @@ namespace CDASimAdapter * @param msg TimeSyncMessage. */ void forward_time_sync_message(tmx::messages::TimeSyncMessage &msg); + /** + * @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); /** * @brief Method to start thread timer for regular interval actions lauched on seperate thread. */ @@ -98,6 +103,11 @@ namespace CDASimAdapter */ void attempt_time_sync(); + /** + * @brief Method to start thread timer for regular interval actions lauched on seperate thread. + */ + void start_sensor_detected_object_detection_thread(); + private: // Simulated location of RSU tmx::utils::Point location; @@ -109,6 +119,7 @@ namespace CDASimAdapter std::unique_ptr connection; // Mutex for configuration parameter thread safety std::mutex _lock; + std::unique_ptr external_object_detection_thread_timer; // Time sync thread to forward time sync messages to PluginClientClockAware V2X-Hub plugins. std::unique_ptr time_sync_timer; // Time sync thread id diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index bdd9d05ed..b6bb55424 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -33,9 +34,8 @@ namespace CDASimAdapter { * @param location Simulationed location of infrastructure. */ explicit CDASimConnection( const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, - const uint sim_v2x_port, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, + const uint sim_v2x_port, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, const tmx::utils::Point &location, const std::string &sensor_json_file_path); - /** * @brief Method to forward v2x message to CARMA Simulation * @param v2x_message string @@ -83,6 +83,12 @@ namespace CDASimAdapter { * @return TimeSyncMessage. */ tmx::messages::TimeSyncMessage consume_time_sync_message() const; + /** + * @brief Method to consume incoming external object message. + * 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; /** * @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, @@ -96,7 +102,7 @@ namespace CDASimAdapter { * @return true if handshake successful and false if handshake unsuccessful. */ bool carma_simulation_handshake(const std::string &simulation_ip, const std::string &infrastructure_id, const uint simulation_registration_port, - const std::string &local_ip, const uint time_sync_port, const uint v2x_port, + const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, const tmx::utils::Point &location); /** @@ -108,7 +114,7 @@ namespace CDASimAdapter { * @param simulation_v2x_port port on which CARMA-Simulation is listening for incoming v2x messages. * @return true if setup is successful and false otherwise. */ - bool setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, + bool setup_udp_connection(const std::string &simulation_ip, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, const uint simulation_v2x_port); /** * @brief Method to attempt to establish connection between CARMA-Simulation and infrastucture. Returns true if succesful @@ -131,19 +137,19 @@ namespace CDASimAdapter { * @param location simulated location of infrastructure hardware. * @return true if handshake successful and false if handshake unsuccessful. */ - std::string get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, - const uint v2x_port, const tmx::utils::Point &location) const; + std::string get_handshake_json(const std::string &infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint simulated_interaction_port, const uint v2x_port, + const tmx::utils::Point &location) const; /** - * @brief Read local file that has the sensor information in JSON format from disk. Populate global sensor json variable with the information. - * @param file_path A string of file location in the host machine. - * @return A reference to the location where the sensors inforation is updated and stored. + * @brief Read Json file specified by the file path from disk, and convert the json into Json::Value object. + * @param file_path A string of file path in the host machine. + * @return A Json::Value object. */ Json::Value read_json_file(const std::string& file_path) const; /** - * @brief Read local file that has the sensor information in JSON format from disk. Populate global sensor json variable with the information. + * @brief Convert the Json string into Json::Value object. * @param json_str A JSON string. - * @return A reference to JSON value. + * @return A Json::Value object. */ Json::Value string_to_json(const std::string &json_str) const; @@ -151,6 +157,7 @@ namespace CDASimAdapter { uint _simulation_registration_port; std::string _infrastructure_id; uint _simulation_v2x_port; + uint _simulated_interaction_port; std::string _local_ip; uint _time_sync_port; uint _v2x_port; @@ -164,6 +171,7 @@ namespace CDASimAdapter { std::shared_ptr immediate_forward_listener; std::shared_ptr message_receiver_publisher; std::shared_ptr time_sync_listener; + std::shared_ptr sensor_detected_object_listener; FRIEND_TEST(TestCARMASimulationConnection, get_handshake_json); FRIEND_TEST(TestCARMASimulationConnection, read_json_file); diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 2c686d170..2149ce8b4 100644 --- a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp @@ -24,7 +24,7 @@ namespace CDASimAdapter { void SetUp() override { // Initialize CARMA Simulation connection with (0,0,0) location. Point location; - connection = std::make_shared("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, location, sensors_file_path); + connection = std::make_shared("127.0.0.1", "1212", 4567, 4678, "127.0.0.1", 1213, 1214, 1215, location, sensors_file_path); } void TearDown() override { @@ -79,7 +79,7 @@ namespace CDASimAdapter { } TEST_F( TestCARMASimulationConnection, setup_upd_connection) { - ASSERT_TRUE(connection->setup_udp_connection("127.0.0.1", "127.0.0.1", 4567, 4568, 4569)); + ASSERT_TRUE(connection->setup_udp_connection("127.0.0.1", "127.0.0.1", 4567, 4568, 4569, 4570)); } TEST_F( TestCARMASimulationConnection, get_handshake_json) { @@ -91,8 +91,8 @@ namespace CDASimAdapter { in_strm.open(sensors_file_path, std::ifstream::binary); if(in_strm.is_open()) { - ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, location), - "{\n \"infrastructureId\" : \"4566\",\n \"location\" : {\n \"x\" : 1000.0,\n \"y\" : 38.954999999999998,\n \"z\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4568,\n \"sensors\" : [\n {\n \"location\" : {\n \"x\" : 0.0,\n \"y\" : 0.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 0.0\n },\n \"sensorId\" : \"SomeID\",\n \"type\" : \"SematicLidar\"\n },\n {\n \"location\" : {\n \"x\" : 1.0,\n \"y\" : 2.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 23.0\n },\n \"sensorId\" : \"SomeID2\",\n \"type\" : \"SematicLidar\"\n }\n ],\n \"timeSyncPort\" : 4567\n}\n"); + ASSERT_EQ(connection->get_handshake_json("4566", "127.0.0.1", 4567, 4568, 4569, location), + "{\n \"infrastructureId\" : \"4566\",\n \"location\" : {\n \"x\" : 1000.0,\n \"y\" : 38.954999999999998,\n \"z\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4569,\n \"sensors\" : [\n {\n \"location\" : {\n \"x\" : 0.0,\n \"y\" : 0.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 0.0\n },\n \"sensorId\" : \"SomeID\",\n \"type\" : \"SematicLidar\"\n },\n {\n \"location\" : {\n \"x\" : 1.0,\n \"y\" : 2.0,\n \"z\" : 0.0\n },\n \"orientation\" : {\n \"pitch\" : 0.0,\n \"roll\" : 0.0,\n \"yaw\" : 23.0\n },\n \"sensorId\" : \"SomeID2\",\n \"type\" : \"SematicLidar\"\n }\n ],\n \"simulatedInteractionPort\" : 4568,\n \"timeSyncPort\" : 4567\n}\n"); } } @@ -100,7 +100,7 @@ namespace CDASimAdapter { Point location; // UDP creation error ASSERT_FALSE(connection->carma_simulation_handshake("", "45", NULL, - "", 45, 45, location)); + "", 45, 45, 45, location)); } TEST_F(TestCARMASimulationConnection, connect) {