From 0963f012d80a2e101608bc166e0cec0d767669fd Mon Sep 17 00:00:00 2001 From: dan-du-car Date: Mon, 17 Jul 2023 23:17:42 +0000 Subject: [PATCH] init --- .devcontainer/docker-compose-vscode.yml | 1 + configuration/amd64/docker-compose.yml | 11 ++ src/tmx/Messages/include/MessageTypes.h | 2 + .../include/simulation/ExternalObject.h | 145 ++++++++++++++++++ .../include/simulation/ObjectEnumTypes.h | 23 +++ .../simulation/PresenceVectorEnumTypes.h | 27 ++++ .../src/simulation/SimulationEnvUtils.h | 5 + .../src/CARMAStreetsPlugin.cpp | 8 + .../src/CARMAStreetsPlugin.h | 2 + .../scripts/send_sim_external_object_udp.py | 117 ++++++++++++++ .../CDASimAdapter/src/CDASimAdapter.cpp | 41 ++++- .../CDASimAdapter/src/CDASimConnection.cpp | 41 +++-- .../src/include/CDASimAdapter.hpp | 10 ++ .../src/include/CDASimConnection.hpp | 16 +- .../test/TestCARMASimulationConnection.cpp | 11 +- .../test/TestSimulationMessages.cpp | 31 ++++ 16 files changed, 468 insertions(+), 23 deletions(-) create mode 100644 src/tmx/Messages/include/simulation/ExternalObject.h create mode 100644 src/tmx/Messages/include/simulation/ObjectEnumTypes.h create mode 100644 src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h create mode 100755 src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py create mode 100644 src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp diff --git a/.devcontainer/docker-compose-vscode.yml b/.devcontainer/docker-compose-vscode.yml index a34dd25c9..24c56ba10 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_EXTERNAL_OBJECT_PORT=7576 - V2X_PORT=8686 - INFRASTRUCTURE_ID=1 - KAFKA_BROKER_ADDRESS=127.0.0.1:9092 diff --git a/configuration/amd64/docker-compose.yml b/configuration/amd64/docker-compose.yml index 6b5188297..088100a81 100755 --- a/configuration/amd64/docker-compose.yml +++ b/configuration/amd64/docker-compose.yml @@ -38,6 +38,17 @@ services: - db environment: - MYSQL_PASSWORD=/run/secrets/mysql_password + - SIMULATION_MODE=true + - SIMULATION_IP=127.0.0.1 + - SIMULATION_REGISTRATION_PORT=6767 + - LOCAL_IP=127.0.0.1 + - TIME_SYNC_TOPIC=time_sync + - TIME_SYNC_PORT=7575 + - SIM_V2X_PORT=5757 + - SIM_EXTERNAL_OBJECT_PORT=7576 + - V2X_PORT=8686 + - INFRASTRUCTURE_ID=1 + - KAFKA_BROKER_ADDRESS=127.0.0.1:9092 secrets: - mysql_password volumes: diff --git a/src/tmx/Messages/include/MessageTypes.h b/src/tmx/Messages/include/MessageTypes.h index a4a84f6f4..2821d10cc 100644 --- a/src/tmx/Messages/include/MessageTypes.h +++ b/src/tmx/Messages/include/MessageTypes.h @@ -54,6 +54,7 @@ static CONSTEXPR const char *MSGTYPE_VEHICLE_STRING = "Vehicle"; static CONSTEXPR const char *MSGTYPE_PEDESTRIAN_STRING = "Pedestrian"; static CONSTEXPR const char *MSGTYPE_PMM_STRING = "Pmm"; static CONSTEXPR const char *MSGTYPE_RADIO_STRING = "Radio"; +static CONSTEXPR const char *MSGTYPE_DETECTED_STRING = "Simulation"; enum MsgSubType { @@ -91,6 +92,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_EXTERNAL_OBJECT_STRING = "ExternalObject"; } /* End namespace messages */ diff --git a/src/tmx/Messages/include/simulation/ExternalObject.h b/src/tmx/Messages/include/simulation/ExternalObject.h new file mode 100644 index 000000000..a7d8586bf --- /dev/null +++ b/src/tmx/Messages/include/simulation/ExternalObject.h @@ -0,0 +1,145 @@ +#ifndef INCLUDE_SIMULATED_EXTERNALOBJECT_H_ +#define INCLUDE_SIMULATED_EXTERNALOBJECT_H_ + +#include +#include +// #include +#include +#include + +namespace tmx +{ + namespace messages + { + namespace simulation + { + /** + * This ExternalObject 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 ExternalObject : public tmx::message + { + public: + ExternalObject(){}; + ExternalObject(const tmx::message_container_type &contents) : tmx::message(contents) {} + ExternalObject(PRESENCE_VECTOR_TYPES presenceVector, uint32_t id, + double confidence, OBJECT_TYPES objectType, bool dynamticObj) + { + set_PresenceVector(presenceVector); + set_Id(id); + set_Confidence(confidence); + set_ObjectType(objectType); + set_DynamticObj(dynamticObj); + }; + ~ExternalObject(){}; + // Message type fpr routing this message through TMX core + static constexpr const char *MessageType = MSGTYPE_DETECTED_STRING; + + // Message sub type for routing this message through TMX core + static constexpr const char *MessageSubType = MSGSUBTYPE_EXTERNAL_OBJECT_STRING; + + /** + *Header contains the frame rest of the fields will use + */ + // sequence ID: consecutively increasing ID + std_attribute(this->msg, uint32_t, HeaderSeq, 0, ); + // Two-integer timestamp that is expressed as: + // # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs') + // # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs') + // # time-handling sugar is provided by the client library + // The seconds component, valid over all int32 values. + std_attribute(this->msg, uint32_t, HeaderTimeSec, 0, ); + // # The nanoseconds component, valid in the range [0, 10e9). + std_attribute(this->msg, uint32_t, HeaderTimeNanoSec, 0, ); + + // A presence vector, this message is used to describe objects coming from potentially different + // sources. The presence vector is used to determine what items are set by the producer. + std_attribute(this->msg, PRESENCE_VECTOR_TYPES, PresenceVector, PRESENCE_VECTOR_TYPES::OBJECT_TYPE_PRESENCE_VECTOR, ); + + // Object id. Matching ids on a topic should refer to the same object within some time period, expanded + std_attribute(this->msg, uint32_t, Id, 0, ); + + // bsm id is of form [0xff, 0xff, 0xff, 0xff]. It is not required. + // uint8[] bsm_id + struct BSMID + { + uint8_t BsmId = 0; + + BSMID() {} + BSMID(std::uint8_t bsmId) : BsmId(bsmId) {} + + static message_tree_type to_tree(BSMID element) + { + message_tree_type treeElement; + treeElement.put("BsmId", element.BsmId); + return treeElement; + } + + static BSMID from_tree(message_tree_type &treeElement) + { + BSMID element; + element.BsmId = treeElement.get("BsmId"); + return element; + } + }; + array_attribute( BSMID, BsmId); + + // Pose of the object within the frame specified in header + // geometry_msgs/PoseWithCovariance pose + // This represents a pose in free space with uncertainty. + std_attribute(this->msg, double, PosePointX, 0, ); + std_attribute(this->msg, double, PosePointY, 0, ); + std_attribute(this->msg, double, PosePointZ, 0, ); + + // This represents an orientation in free space in quaternion form. + std_attribute(this->msg, double, PoseQuaternionX, 0, ); + std_attribute(this->msg, double, PoseQuaternionY, 0, ); + std_attribute(this->msg, double, PoseQuaternionZ, 0, ); + std_attribute(this->msg, double, PoseQuaternionW, 0, ); + + // Row-major representation of the 6x6 covariance matrix + // # The orientation parameters use a fixed-axis representation. + // # In order, the parameters are: + // # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) + std_attribute(this->msg, double, Covariance, 0, ); + + // #Average velocity of the object within the frame specified in header + // geometry_msgs/TwistWithCovariance velocity + + // #Instantaneous velocity of an object within the frame specified in header + // geometry_msgs/TwistWithCovariance velocity_inst + + // #The size of the object aligned along the axis of the object described by the orientation in pose + // #Dimensions are specified in meters + /** + * # This represents a vector in free space. + # It is only meant to represent a direction. Therefore, it does not + # make sense to apply a translation to it (e.g., when applying a + # generic rigid transformation to a Vector3, tf2 will only apply the + # rotation). If you want your data to be translatable too, use the + # geometry_msgs/Point message instead. + */ + // geometry_msgs/Vector3 size + std_attribute(this->msg, double, SizeX, 0, ); + std_attribute(this->msg, double, SizeY, 0, ); + std_attribute(this->msg, double, SizeZ, 0, ); + + // #Confidence [0,1] + std_attribute(this->msg, double, Confidence, 0, ); + + // #describes a general object type as defined in this message + std_attribute(this->msg, OBJECT_TYPES, ObjectType, OBJECT_TYPES::UNKNOWN, ); + + // # Binary value to show if the object is static or dynamic (1: dynamic, 0: static) + std_attribute(this->msg, bool, DynamticObj, false, ); + + // Predictions for the object. It is not required. + // carma_perception_msgs/PredictedState[] predictions + }; + + } + } + +}; // namespace tmx +#endif diff --git a/src/tmx/Messages/include/simulation/ObjectEnumTypes.h b/src/tmx/Messages/include/simulation/ObjectEnumTypes.h new file mode 100644 index 000000000..15d443a48 --- /dev/null +++ b/src/tmx/Messages/include/simulation/ObjectEnumTypes.h @@ -0,0 +1,23 @@ +#ifndef INCLUDE_SIMULATED_OBJECT_TYPE_H_ +#define INCLUDE_SIMULATED_OBJECT_TYPE_H_ + +namespace tmx +{ + namespace messages + { + namespace simulation + { + // #used for object type + enum OBJECT_TYPES + { + UNKNOWN = 0, + SMALL_VEHICLE = 1, + LARGE_VEHICLE = 2, + MOTORCYCLE = 3, + PEDESTRIAN = 4 + }; + } + } + +}; // namespace tmx +#endif \ No newline at end of file diff --git a/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h b/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h new file mode 100644 index 000000000..8515fc728 --- /dev/null +++ b/src/tmx/Messages/include/simulation/PresenceVectorEnumTypes.h @@ -0,0 +1,27 @@ +#ifndef INCLUDE_SIMULATED_PRESENCE_VECTOR_H_ +#define INCLUDE_SIMULATED_PRESENCE_VECTOR_H_ + +namespace tmx +{ + namespace messages + { + namespace simulation + { + enum PRESENCE_VECTOR_TYPES + { + ID_PRESENCE_VECTOR = 1, + POSE_PRESENCE_VECTOR = 2, + VELOCITY_PRESENCE_VECTOR = 4, + VELOCITY_INST_PRESENCE_VECTOR = 8, + SIZE_PRESENCE_VECTOR = 16, + CONFIDENCE_PRESENCE_VECTOR = 32, + OBJECT_TYPE_PRESENCE_VECTOR = 64, + BSM_ID_PRESENCE_VECTOR = 128, + DYNAMIC_OBJ_PRESENCE = 256, + PREDICTION_PRESENCE_VECTOR = 512 + }; + } + } + +}; // namespace tmx +#endif \ No newline at end of file diff --git a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h index 1b04ffe11..08b93e49d 100644 --- a/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h +++ b/src/tmx/TmxUtils/src/simulation/SimulationEnvUtils.h @@ -41,6 +41,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_EXTERNAL_OBJECT_PORT = "SIM_EXTERNAL_OBJECT_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/src/CARMAStreetsPlugin.cpp b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp index 71703bd59..5ebdeb5e0 100755 --- a/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp +++ b/src/v2i-hub/CARMAStreetsPlugin/src/CARMAStreetsPlugin.cpp @@ -25,6 +25,7 @@ CARMAStreetsPlugin::CARMAStreetsPlugin(string name) : AddMessageFilter < tsm2Message > (this, &CARMAStreetsPlugin::HandleMobilityPathMessage); AddMessageFilter < MapDataMessage > (this, &CARMAStreetsPlugin::HandleMapMessage); AddMessageFilter < SrmMessage > (this, &CARMAStreetsPlugin::HandleSRMMessage); + AddMessageFilter < simulation::ExternalObject> (this, &CARMAStreetsPlugin::HandleSimulatedExternalMessage ); SubscribeToMessages(); @@ -626,6 +627,13 @@ void CARMAStreetsPlugin::SubscribeSSMKafkaTopic(){ } } + +void CARMAStreetsPlugin::HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg) +{ + PLOG(logINFO) << "HandleSimulatedExternalMessage called." < #include #include "JsonToJ2735SSMConverter.h" +#include @@ -48,6 +49,7 @@ class CARMAStreetsPlugin: public PluginClient { void HandleMobilityOperationMessage(tsm3Message &msg, routeable_message &routeableMsg); void HandleMobilityPathMessage(tsm2Message &msg, routeable_message &routeableMsg); void HandleBasicSafetyMessage(BsmMessage &msg, routeable_message &routeableMsg); + void HandleSimulatedExternalMessage(simulation::ExternalObject &msg, routeable_message &routeableMsg); /** * @brief Subscribe to MAP message broadcast by the MAPPlugin. This handler will be called automatically whenever the MAPPlugin is broadcasting a J2735 MAP message. * @param msg The J2735 MAP message received from the internal diff --git a/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py b/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py new file mode 100755 index 000000000..e2ec2d4de --- /dev/null +++ b/src/v2i-hub/CDASimAdapter/scripts/send_sim_external_object_udp.py @@ -0,0 +1,117 @@ + +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":{ + "is_simulation": False, + "datum": "", + "proj_string": "", + "sensor_x": 0.0, + "sensor_y": 0.0, + "sensor_z": 0.0, + "infrastructure_id": "", + "sensor_id": "" + }, + "header": { + "seq": 0, + "stamp": { + "secs": 0, + "nsecs": 0 + } + }, + "id": 0, + "pose": { + "pose": { + "position": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "orientation": { + "x": 0.0, + "y": 0.0, + "z": 0.0, + "w": 0.0 + } + }, + "covariance": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ] + }, + "velocity": { + "twist": { + "linear": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "angular": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + } + }, + "covariance": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ] + }, + "size": { + "x": 0.0, + "y": 0.0, + "z": 0.0 + }, + "confidence": 0.0, + "object_type": "", + "dynamic_obj": False + } + + 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 a036ff6f2..0d0bd5269 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimAdapter.cpp @@ -40,6 +40,7 @@ namespace CDASimAdapter{ start_time_sync_thread_timer(); start_amf_msg_thread(); start_binary_msg_thread(); + start_external_object_detection_thread(); } } @@ -77,6 +78,11 @@ namespace CDASimAdapter{ } + void CDASimAdapter::forward_simulated_external_message(tmx::messages::simulation::ExternalObject &msg) { + PLOG(logDEBUG1) << "Sending Simulated ExternalObject 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); @@ -84,6 +90,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)); + uint external_object_detection_port = std::stoul(sim::get_sim_config(sim::SIM_EXTERNAL_OBJECT_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)); uint infrastructure_id = std::stoul(sim::get_sim_config(sim::INFRASTRUCTURE_ID));; @@ -96,11 +103,11 @@ namespace CDASimAdapter{ } if ( connection ) { connection.reset(new CDASimConnection( simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, v2x_port, location )); + time_sync_port, external_object_detection_port, v2x_port, location )); } else { connection = std::make_unique(simulation_ip, infrastructure_id, simulation_registration_port, sim_v2x_port, local_ip, - time_sync_port, v2x_port, location); + time_sync_port, external_object_detection_port, v2x_port, location); } } catch (const TmxException &e) { @@ -156,6 +163,36 @@ namespace CDASimAdapter{ } } + void CDASimAdapter::start_external_object_detection_thread() { + PLOG(logDEBUG) << "Creating Thread Timer for simulated external object" << std::endl; + try + { + if(!external_bject_detection_thread_timer) + { + external_bject_detection_thread_timer = std::make_unique(); + } + external_bject_detection_thread_timer->AddPeriodicTick([this](){ + PLOG(logDEBUG1) << "Listening for External Object Message from CDASim." << std::endl; + auto msg = connection->consume_external_object_message(); + if ( !msg.is_empty()) { + PLOG(logDEBUG1) << "Consumed External Object Message: " << msg<forward_simulated_external_message(msg); + PLOG(logDEBUG1) << "External Object Message Forwarded to CARMAStreetsPlugin!" << std::endl; + } + else + { + PLOG(logDEBUG1) << "CDASim connection has not yet received an simulated external message!" << std::endl; + } + }//End lambda + , std::chrono::milliseconds(100)); + external_bject_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 7349bda38..7d7722440 100644 --- a/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp +++ b/src/v2i-hub/CDASimAdapter/src/CDASimConnection.cpp @@ -5,10 +5,10 @@ using namespace tmx::utils; namespace CDASimAdapter{ CDASimConnection::CDASimConnection(const std::string &simulation_ip, const uint 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 external_object_detection_port, const uint v2x_port, const WGS84Point &location) : _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), + _simulation_v2x_port(sim_v2x_port), _local_ip(local_ip), _time_sync_port(time_sync_port), _external_object_detection_port(external_object_detection_port),_v2x_port(v2x_port), _location(location) { 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, _external_object_detection_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,_external_object_detection_port, _v2x_port, _simulation_v2x_port )) { _connected = false; return _connected; } @@ -33,7 +33,7 @@ namespace CDASimAdapter{ return _connected; } - std::string CDASimConnection::get_handshake_json(const uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint v2x_port, + std::string CDASimConnection::get_handshake_json(const uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint external_object_detection_port, const uint v2x_port, const WGS84Point &location) const { @@ -44,6 +44,7 @@ namespace CDASimAdapter{ message["infrastructureId"] = infrastructure_id; message["rxMessagePort"] = v2x_port; message["timeSyncPort"] = time_sync_port; + message["ExternalObjectDetectionPort"] = external_object_detection_port; message["location"]["latitude"] = location.Latitude; message["location"]["longitude"] = location.Longitude; message["location"]["elevation"] = location.Elevation; @@ -53,13 +54,13 @@ namespace CDASimAdapter{ } bool CDASimConnection::carma_simulation_handshake(const std::string &simulation_ip, const uint 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 external_object_detection_port, const uint v2x_port, const WGS84Point &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, external_object_detection_port, v2x_port, location); try { @@ -75,7 +76,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 external_object_detection_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 @@ -92,12 +93,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(external_object_detection_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); + external_object_listener = std::make_shared (local_ip, external_object_detection_port); } catch (const UdpClientRuntimeError &e) { PLOG(logERROR) << "Encountered UDPClient Runtime error during UDP connection initialization : " << e.what() << std::endl; @@ -125,6 +128,22 @@ namespace CDASimAdapter{ } + tmx::messages::simulation::ExternalObject CDASimConnection::consume_external_object_message() const + { + tmx::messages::simulation::ExternalObject externalObj; + externalObj.clear(); + if(external_object_listener) + { + std::string str_msg = consume_server_message(external_object_listener); + externalObj.set_contents(str_msg); + } + else + { + throw std::runtime_error("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); diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp index 6de71bcd6..88c5ec032 100644 --- a/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp +++ b/src/v2i-hub/CDASimAdapter/src/include/CDASimAdapter.hpp @@ -99,6 +99,11 @@ namespace CDASimAdapter { * @param msg TimeSyncMessage. */ void forward_time_sync_message(tmx::messages::TimeSyncMessage &msg); + /** + * @brief Forward simulated external object message to TMX message bus for other V2X-Hub Plugin + * @param msg simulation::ExternalObject. + */ + void forward_simulated_external_message(tmx::messages::simulation::ExternalObject &msg); /** * @brief Method to start thread timer for regular interval actions lauched on seperate thread. */ @@ -107,6 +112,10 @@ namespace CDASimAdapter { * @brief Method to consume time sychrononization from CDASimConnection and forward to tmx core and CARMA Streets */ void attempt_time_sync(); + /** + * @brief Method to start thread timer for regular interval actions lauched on seperate thread. + */ + void start_external_object_detection_thread(); private: @@ -115,6 +124,7 @@ namespace CDASimAdapter { std::unique_ptr connection; std::mutex _lock; std::unique_ptr thread_timer; + std::unique_ptr external_bject_detection_thread_timer; int time_sync_tick_id; std::unique_ptr amf_thread_timer; std::unique_ptr binary_thread_timer; diff --git a/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp b/src/v2i-hub/CDASimAdapter/src/include/CDASimConnection.hpp index ab6771609..fec88be2d 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,7 +34,7 @@ namespace CDASimAdapter { * @param producer Kafka Producer for forwarding time synchronization messages. */ explicit CDASimConnection( const std::string &simulation_ip, const uint 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 external_object_detection_port, const uint v2x_port, const tmx::utils::WGS84Point &location); /** @@ -83,6 +84,11 @@ namespace CDASimAdapter { * @return TimeSyncMessage. */ tmx::messages::TimeSyncMessage consume_time_sync_message() const; + /** + * @brief Method to consume incoming external object message. + * @return simulation::ExternalObject. + */ + tmx::messages::simulation::ExternalObject consume_external_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 uint 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 external_object_detection_port, const uint v2x_port, const tmx::utils::WGS84Point &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 external_object_detection_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,13 +137,14 @@ 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 uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, + std::string get_handshake_json(const uint infrastructure_id, const std::string &local_ip, const uint time_sync_port, const uint external_object_detection_port, const uint v2x_port, const tmx::utils::WGS84Point &location) const; std::string _simulation_ip; uint _simulation_registration_port; uint _infrastructure_id; uint _simulation_v2x_port; + uint _external_object_detection_port; std::string _local_ip; uint _time_sync_port; uint _v2x_port; @@ -150,6 +157,7 @@ namespace CDASimAdapter { std::shared_ptr immediate_forward_listener; std::shared_ptr message_receiver_publisher; std::shared_ptr time_sync_listener; + std::shared_ptr external_object_listener; FRIEND_TEST(TestCARMASimulationConnection, get_handshake_json); }; diff --git a/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp b/src/v2i-hub/CDASimAdapter/test/TestCARMASimulationConnection.cpp index 442c83142..eaded6b38 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 and mock kafka producer. WGS84Point location; - connection = std::make_shared("127.0.0.1", 1212, 4567, 4678, "127.0.0.1", 1213, 1214, location); + connection = std::make_shared("127.0.0.1", 1212, 4567, 4678, "127.0.0.1", 1213, 1214, 1215, location); } 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) { @@ -87,16 +87,15 @@ namespace CDASimAdapter { location.Elevation = 1000; location.Latitude = 38.955; location.Longitude = -77.149; - - ASSERT_EQ(connection->get_handshake_json(4566, "127.0.0.1", 4567, 4568, location), - "{\n \"infrastructureId\" : 4566,\n \"location\" : {\n \"elevation\" : 1000.0,\n \"latitude\" : 38.954999999999998,\n \"longitude\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4568,\n \"timeSyncPort\" : 4567\n}\n"); + ASSERT_EQ(connection->get_handshake_json(4566, "127.0.0.1", 4567, 4568, 4569, location), + "{\n \"ExternalObjectDetectionPort\" : 4568,\n \"infrastructureId\" : 4566,\n \"location\" : {\n \"elevation\" : 1000.0,\n \"latitude\" : 38.954999999999998,\n \"longitude\" : -77.149000000000001\n },\n \"rxMessageIpAddress\" : \"127.0.0.1\",\n \"rxMessagePort\" : 4569,\n \"timeSyncPort\" : 4567\n}\n"); } TEST_F( TestCARMASimulationConnection, carma_simulation_handshake) { WGS84Point location; // UDP creation error ASSERT_FALSE(connection->carma_simulation_handshake("", 45, NULL, - "", 45, 45, location)); + "", 45, 45, 45, location)); } TEST_F(TestCARMASimulationConnection, connect) { diff --git a/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp new file mode 100644 index 000000000..af6ce9f77 --- /dev/null +++ b/src/v2i-hub/CDASimAdapter/test/TestSimulationMessages.cpp @@ -0,0 +1,31 @@ +#include +#include +#include +#include + +using namespace std; +using namespace tmx; +using namespace tmx::messages; + +TEST(SimulationMessages, ExternalObjectToRoutableMessage) +{ + simulation::ExternalObject externalObj; + string expectedStr = "{\"metadata\":{\"is_simulation\":false,\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":0.0,\"sensor_y\":0.0,\"sensor_z\":0.0,\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":0,\"stamp\":{\"secs\":0,\"nsecs\":0}},\"id\":0,\"pose\":{\"pose\":{\"position\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"orientation\":{\"x\":0.0,\"y\":0.0,\"z\":0.0,\"w\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"velocity\":{\"twist\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"covariance\":[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]},\"size\":{\"x\":0.0,\"y\":0.0,\"z\":0.0},\"confidence\":0.0,\"object_type\":\"\",\"dynamic_obj\":false}"; + externalObj.set_contents(expectedStr); + ASSERT_EQ(expectedStr, externalObj.to_string()); + ASSERT_EQ("ExternalObject", std::string(simulation::ExternalObject::MessageSubType)); + ASSERT_EQ("Simulation", std::string(simulation::ExternalObject::MessageType)); + tmx::routeable_message routeableMsg; + routeableMsg.initialize(externalObj, "CDASimAdapter", 0 , IvpMsgFlags_None); + string output = "{\"metadata\":{\"is_simulation\":\"false\",\"datum\":\"\",\"proj_string\":\"\",\"sensor_x\":\"0.0\",\"sensor_y\":\"0.0\",\"sensor_z\":\"0.0\",\"infrastructure_id\":\"\",\"sensor_id\":\"\"},\"header\":{\"seq\":\"0\",\"stamp\":{\"secs\":\"0\",\"nsecs\":\"0\"}},\"id\":\"0\",\"pose\":{\"pose\":{\"position\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"orientation\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\",\"w\":\"0.0\"}},\"covariance\":[\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\"]},\"velocity\":{\"twist\":{\"linear\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"angular\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"}},\"covariance\":[\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\",\"0.0\"]},\"size\":{\"x\":\"0.0\",\"y\":\"0.0\",\"z\":\"0.0\"},\"confidence\":\"0.0\",\"object_type\":\"\",\"dynamic_obj\":\"false\"}"; + ASSERT_EQ(output, routeableMsg.get_payload_str()); + ASSERT_EQ("json", routeableMsg.get_encoding()); + ASSERT_EQ(0, routeableMsg.get_flags()); + auto current_time_mill = boost::chrono::duration_cast(boost::chrono::system_clock::now().time_since_epoch()).count(); + ASSERT_NEAR(current_time_mill, routeableMsg.get_timestamp(), 1000); + ASSERT_NEAR(current_time_mill, routeableMsg.get_millisecondsSinceEpoch(), 1000); + ASSERT_EQ(0, routeableMsg.get_sourceId()); + ASSERT_EQ("CDASimAdapter", routeableMsg.get_source()); + ASSERT_EQ("ExternalObject",routeableMsg.get_subtype()); + ASSERT_EQ("Detected", routeableMsg.get_type()); +} \ No newline at end of file